ORGANISATION DE LA THÈSE contrôle sont finalement illustrées au travers de plusieurs scénarios expérimentaux ex-vivo utilisant des caméras ou l'échographie 3D comme retour visuel.

Chapitre 5: Nous considérons le problème des mouvements du patient pendant la procédure d'insertion d'aiguille. Nous présentons d'abord une vue d'ensemble des techniques de compensation de mouvement pendant l'insertion d'une aiguille. Notre approche de contrôle introduite dans le chapitre 4 est ensuite étendue et nous exploitons la méthode de mise à jour de modèle proposée dans le chapitre 3 afin de se charger de l'insertion d'une aiguille dans des tissus subissant des mouvements latéraux. Nous fournissons les résultats expérimentaux obtenus en utilisant notre approche de contrôle pour guider l'insertion d'une aiguille dans un fantôme constitué de tissus mous en mouvement. Ces expériences ont été réalisées en utilisant plusieurs modalités de retour d'information, fournies par un capteur d'efforts, un capteur électromagnétique et l'échographie 2D.

Conclusion:

Finalement nous concluons cette thèse et présentons des perspectives pour de possibles extensions et applications.

Acknowledgments

There are many people that I want to thank for making the realization of this thesis such a great experience. I have never really felt at ease telling my life and my feelings, so I will keep it short and hope to make it meaningful.

I would first like to deeply thank Alexandre Krupa and Marie Babel for the supervision of this work. I greatly appreciated the freedom and trust they gave me to conduct my research as well as their guidance, feedback and support at the different stages of this thesis: from peaceful periods to harder times when I was late just a few hours before submission deadlines.

I am really grateful to Philippe Poignet and Nicolas Andreff for the time they took to review my manuscript within the tight schedule that was given to them. I would also like to thank them along with Bernard Bayle and Sarthak Misra for being members of my thesis committee and for providing interesting feedback and discussions on my work to orient my future research.

Additional thanks go to Sarthak for the great opportunity he gave me to spend some time in the Netherlands at the Surgical Robotics Laboratory. I would also like to thank the other members of this lab for their welcome, and especially Navid for the time we spent working together.

I would like to express my gratitude to François Chaumette for introducing me the Lagadic team months before I started this thesis, which gave me the desire to work there. Many thanks go to all the members and ex-members of this team that I had the pleasure to meet during these years at IRISA. A bit for their help with my work, but mostly for the great atmosphere during all the coffee/tea/lunch breaks and the various out-of-office activities. The list has become too long to mention everyone separately, but working on this thesis during these years would not have been such a great pleasure without all of the moments we spent together, and I hope this can continue.

I would also like to thank all my friends from Granville, Cachan or Rennes, who all contributed to the success of this thesis, consciously or not. A special thank you to Rémi, for various reasons in general, but more specifically here for being among the very few people to proof-read a part of this manuscript and give some feedback.

Finally, my warmest thanks go to my family for their continuous support and encouragements during all this time, before and during the thesis.

Les procédures cliniques mini-invasives se sont largement étendues durant ce dernier siècle. La méthode traditionnellement utilisées pour traiter un patient a longtemps été de recourir à la chirurgie ouverte, qui consiste à faire de larges incisions dans le corps pour pouvoir observer et manipuler ses structures internes. Le taux de succès de ce genre d'approche est tout d'abord limité par les lourdes modifications apportées au corps du patient, qui mettent du temps à guérir et peuvent entrainer des complications après l'opération. Il s'ensuit également un risque accru d'infection dû à l'exposition des tissues internes à l'environnement extérieur. Au contraire, les procédures mini-invasives ne requièrent qu'un nombre limité de petites incisions pour accéder aux organes. Le bien-être général du patient est donc amélioré grâce à la réduction des douleurs post-opératoires et la limitation de la présence de larges cicatrices. Le temps de rétablissement des patients est également grandement réduit [EGH + 13], en même temps que les risques d'infection [START_REF] Gandaglia | Effect of minimally invasive surgery on the risk for surgical site infections: Results from the national surgical quality improvement program (nsqip) database[END_REF], ce qui conduit à de meilleurs taux de succès des opérations et une réduction des coûts pour les hôpitaux.

Lorsque la chirurgie ouverte était nécessaire avant l'introduction de l'imagerie médicale, diagnostique et traitement pouvaient faire partie d'une seule et même opération, pour tout d'abord voir les organes et ensuite planifier et effectuer l'opération nécessaire. Les rayons X ont été parmi les premiers moyens découverts pour permettre l'obtention d'une vue anatomique de l'intérieur du corps sans nécessiter de l'ouvrir. Plusieurs modalités d'imagerie ont depuis été développées et améliorées, parmi lesquelles la tomodensitométrie (TDM) [START_REF] Hounsfield | Computerized transverse axial scanning (tomography): Part 1. description of system[END_REF], l'imagerie par résonance magnétique (IRM) [START_REF] Lauterbur | Image formation by induced local interactions: Examples employing nuclear magnetic resonance[END_REF] et l'échographie [START_REF] Wild | Application of echo-ranging techniques to the determination of structure of biological tissues[END_REF] sont maintenant largement utilisées dans le domaine médical.

Au-delà des capacités de diagnostic accrues qu'elle offre, l'imagerie médicale a joué un rôle important dans le développement de l'approche chirurgicale mini-invasive. Observer l'intérieur du corps est nécessaire au succès d'une intervention chirurgicale, afin de voir les tissus d'intérêt et la position des outils chirurgicaux. De part la nature même de la chirurgie miniinvasive, une ligne de vue directe sur l'intérieur du corps n'est pas possible et il est donc nécessaire d'utiliser d'autres moyens d'observation visuelle, tels que l'insertion d'endoscope ou des techniques d'imagerie anatomique.

i RÉSUMÉ EN FRANÇAIS

Chaque technique a ses propres avantages et inconvénients. Les endoscopes utilisent des caméras, ce qui offre une vue similaire à un oeil humain. Les images sont donc faciles à interpréter, cependant il n'est pas possible de voir à travers les tissus. À l'opposé, l'imagerie anatomique permet de visualiser l'intérieur des tissus, mais un entrainement spécifique des médecins est nécessaire pour l'interprétation des images obtenues. La tomodensitométrie utilise des rayons X, qui sont des radiations ionisantes, ce qui limite néanmoins le nombre d'images qui peuvent être acquises afin de ne pas exposer le patient à des doses de rayonnement trop importantes [SBA + 09]. L'équipe médicale doit également rester en dehors de la salle où se trouve le scanner pendant la durée d'acquisition. D'un autre côté l'IRM utilise des radiations non-invasives et fournit également des images de haute qualité, avec une grande résolution et un large champ de vue. Cependant ces deux modalités imposent de sévères contraintes, telles qu'un long temps nécessaire pour obtenir une image ou un équipement coûteux et encombrant qui limite l'accès au patient. Dans ce contexte l'échographie est une modalité de choix grâce à sa capacité à fournir une visualisation en temps réel des tissus et des outils chirurgicaux en mouvement. De plus, elle est non-invasive et ne requière que des scanners légers et des sondes facilement manipulables.

Des outils longilignes sont souvent utilisés pour les procédures miniinvasives afin d'être insérés à travers de petites incisions réalisées à la surface du patient. En particulier les aiguilles ont été utilisées depuis longtemps pour extraire ou injecter des substances directement dans le corps. Elles procurent un accès aux structures internes tout en ne laissant qu'une faible marque dans les tissus. Pour cette raison elles sont des outils de premier choix pour une invasion minimale et permettent d'atteindre de petites structures dans des régions profondes du corps. Cependant les aiguille fines peuvent présenter un certain niveau de flexibilité, ce qui rend difficile le contrôle précis de leur trajectoire. Couplé au fait qu'une sonde échographique doit être manipulée en même temps que le geste d'insertion d'aiguille, la procédure d'insertion peut rapidement devenir une tâche ardue qui requière un entrainement spécifique des cliniciens. En conséquence, le guidage robotisé des aiguilles est devenu un vaste sujet de recherche pour fournir un moyen de faciliter l'intervention des cliniciens et augmenter la précision générale de la procédure.

La robotique médicale a pour but de manière générale de concevoir et contrôler des systèmes mécatroniques afin d'assister les cliniciens dans leur tâches. L'objectif principal étant d'améliorer la précision, la sécurité et la répétabilité des opérations tout en réduisant leur durée [START_REF] Taylor | Medical robotics in computer-integrated surgery[END_REF]. Cela peut grandement bénéficier aux procédures d'insertion d'aiguille en particulier, pour lesquelles la précision est bien souvent cruciale pour éviter les erreurs de ciblage et la répétition inutile d'insertions. L'intégration d'un système robotique dans les blocs opératoires reste un grand défi en raison des contraintes cliniques et de l'acceptation du dispositif technique par le personnel médical. Parmi les différentes conceptions qui ont été proposées, certains sysii MOTIVATIONS CLINIQUES tèmes présentent plus de chances de succès que d'autres. De tels systèmes doivent offrir soit une assistance au chirurgien sans modifier de manière significative le déroulement de l'opération soit des bénéfices clairs à la fois sur la réussite de l'opération et sur les conditions opératoires du chirurgien. C'est le cas par exemple des systèmes d'amélioration des images médicales ou de suppression des tremblements ou encore des systèmes télé-opérés. Pour les procédures d'insertion d'aiguille, cela consisterait principalement à fournir un monitoring en temps réel du déroulement de l'insertion ainsi qu'un système robotique entre le patient et la main du chirurgien servant à assister le processus d'insertion. À cet égard, un système robotique guidé par échographie est un bon choix pour fournir une imagerie intra-opératoire en temps réel et une assistance pendant l'opération.

Motivations cliniques

Les aiguilles sont largement utilisées dans une grande variété d'actes médicaux pour l'injection de substances ou le prélèvement d'échantillons de tissus ou de fluides directement à l'intérieur du corps. Alors que certaines procédures ne nécessitent pas un placement précis de la pointe de l'aiguille, comme les injections intramusculaires, le résultat des opérations sensibles dépend grandement de la capacité à atteindre une cible précise à l'intérieur du corps. Dans la suite nous présentons quelques applications pour lesquelles un ciblage précis et systématique est crucial pour éviter des conséquences dramatiques et qui pourraient grandement bénéficier d'une assistance robotisée.

Biopsies pour le diagnostic de cancer

Le cancer est devenu une des causes majeures de mortalité dans le monde avec 8.2 millions de décès dus au cancer estimés à travers le monde en 2015 [TBS + 15]. Parmi les nombreuses variétés de cancer, le cancer de la prostate est l'un des plus diagnostiqués parmi les hommes et le cancer du sein parmi les femmes, le cancer du poumon étant aussi une cause majeure de décès pour les deux. Cependant la détection précoce des cancers peut améliorer la probabilité de succès d'un traitement et diminuer le taux de mortalité. Indépendamment du type de tumeur, la biopsie est la méthode de diagnostic traditionnellement utilisée pour confirmer la malignité de tissus suspects. Elle consiste à utiliser une aiguille pour prélever un petit échantillon de tissu à une position bien définie à des fins d'analyse. Le placement précis de l'aiguille est d'une importance capitale dans ce genre de procédure afin d'éviter une erreur de diagnostic due au prélèvement de tissus sains autour de la région suspectée. Les insertions manuelles peuvent donner des résultats variables qui dépendent du clinicien effectuant l'opération. Le guidage robotique de l'aiguille a donc le potentiel de grandement améliorer les performances des biopsies. Un retour échographique est souvent utilisé, par iii RÉSUMÉ EN FRANÇAIS exemple pour le diagnostic du cancer de la prostate [START_REF] Kaye | Robotic ultrasound and needle guidance for prostate cancer manage-ment: review of the contemporary literature[END_REF]. La tomodensitométrie est également un bon choix pour le cancer du poumon et un système robotique est d'une grande aide afin de compenser les mouvements de respiration [ZTK + 13]. Les systèmes robotiques peuvent également être utilisés afin de maintenir et modifier la position des tissus pour aligner une tumeur potentielle avec l'aiguille, particulièrement dans le cas de biopsies du cancer du sein [START_REF] Mallapragada | Robotassisted real-time tumor manipulation for breast biopsy[END_REF].

Curiethérapie

La curiethérapie a prouvé être un moyen efficace pour traiter le cancer de la prostate [GBS + 01]. Elle consiste à placer de petits grains radioactifs dans la tumeur à détruire. Cette procédure nécessite le placement précis et uniforme d'une centaine de grains, ce qui peut prendre du temps et requière une grande précision. Les conséquences d'un mauvais placement peuvent être dramatiques par la destruction de structures sensibles alentours, comme la vessie, le rectum, la vésicule séminale ou l'urètre. L'insertion est habituellement effectuée sous échographie trans-rectale, ce qui peut permettre d'utiliser un système robotisé pour accomplir des insertions précises et répétées sous guidage échographique [START_REF] Hungr | A 3d ultrasound robotic prostate brachytherapy system with prostate motion tracking[END_REF] [SSK + 12] [START_REF] Kaye | Robotic ultrasound and needle guidance for prostate cancer manage-ment: review of the contemporary literature[END_REF]. L'IRM est également couramment utilisée et fait l'objet de recherche pour une utilisation avec un système robotique [START_REF] Seifabadi | Toward teleoperated needle steering under continuous mri guidance for prostate percutaneous interventions[END_REF].

Cancer du foie

Après le cancer du poumon, le cancer du foie est la cause majeure de décès dus au cancer chez l'homme, avec environ 500000 décès chaque année [TBS + 15]. L'ablation par radiofréquence est la principale modalité thérapeutique utilisée pour effectuer une ablation de tumeur du foie [START_REF] Lencioni | Percutaneous image-guided radiofrequency ablation of liver tumors[END_REF]. Une sonde d'ablation, apparentée à une aiguille, est insérée dans le foie et génère de la chaleur pour détruire localement les tissus. Guider précisément la sonde sous guidage visuel peut éviter la destruction inutile de trop de tissus. Les biopsies du foie peuvent également être effectuées en utilisant des aiguilles de ponction percutanée [START_REF] Grant | Guidelines on the use of liver biopsy in clinical practice[END_REF]. Utiliser un guidage robotisé sous modalité échographique pourrait permettre d'éviter de multiple insertions qui augmentent les saignements hépatiques et peuvent avoir de graves conséquences.

Contributions

Dans cette thèse nous traitons du contrôle automatique d'un système robotique pour l'insertion d'une aiguille flexible dans des tissus mous sous guidage échographique. Traiter ce sujet nécessite de considérer plusieurs points. Tout d'abord l'interaction entre l'aiguille et les tissus doit être modélisée afin iv CONTRIBUTIONS de pouvoir prédire l'effet du système robotique sur l'état de la procédure d'insertion. Le modèle doit être capable de représenter les différents aspects de l'insertion et être à la fois suffisamment simple pour être utilisé en temps réel. Une méthode de contrôle doit également être conçue pour permettre de diriger la pointe de l'aiguille vers sa cible tout en maintenant la sécurité de l'opération. Le ciblage précis est rendu difficile par le fait que les tissus biologiques peuvent présenter une grande variété de comportements. Guider l'aiguille introduit aussi nécessairement une certaine quantité de dommages aux tissus, de telle sorte qu'un compromis doit être choisi entre le succès du ciblage et la réduction des dommages. Les mouvements physiologiques du patient peuvent également être une source importante de mouvement de la région ciblée et doivent aussi être pris en compte pour éviter d'endommager les tissus ou l'aiguille. Finalement la détection fiable de l'aiguille dans les images échographiques est un pré-requis pour pouvoir guider l'aiguille dans la bonne direction. Cependant cette tâche est rendue difficile par la faible qualité de la modalité échographique. Afin de relever ces défis, nous apportons plusieurs contributions dans cette thèse, qui sont :

• Deux modèles 3D de l'interaction entre une aiguille flexible à pointe biseautée et des tissus mous. Ces modèles sont conçus pour permettre un calcul en temps réel et fournir une représentation 3D de l'ensemble du corps de l'aiguille pendant son insertion dans des tissus en mouvement.

• Une méthode d'estimation des mouvements latéraux des tissus en utilisant uniquement des mesures disponibles sur le corps de l'aiguille.

• Une méthode de suivi d'aiguille flexible dans des volumes échographiques 3D qui prend en compte les artefacts inhérents à la modalité échographique.

• La conception d'une approche de contrôle pour un système robotique insérant une aiguille flexible dans des tissus mous. Cette approche a été développée de manière à être facilement adaptable à n'importe quels composants matériels, que ce soit le type d'aiguille, le système robotique utilisé pour le contrôle des mouvements de l'aiguille ou la modalité de retour utilisée pour obtenir des informations sur l'aiguille. Elle permet également de considérer des stratégies de contrôle hybrides, comme la manipulation des mouvements latéraux appliqués à la base de l'aiguille ou le guidage de la pointe de l'aiguille exploitant une géométrie asymétrique de cette pointe.

• La validation ex-vivo des méthodes proposées en utilisant diverses plateformes expérimentales et différents scénarios afin d'illustrer la flexibilité de notre approche de commande pour différents cas d'insertion d'aiguille.

v RÉSUMÉ EN FRANÇAIS

Organisation de la thèse

Le contenu de chaque chapitre de cette thèse est à présent détaillé dans la suite.

Chapitre 1: Nous présentons le contexte clinique et scientifique dans lequel s'inscrit cette thèse. Nous définissons également nos objectifs principaux et présentons les différents défis associés. Le matériel utilisé dans les différentes expériences effectuées est également présenté.

Chapitre 2: Nous présentons une vue d'ensemble des modèles d'interaction aiguille/tissus. Un état de l'art des différentes familles de modèles est tout d'abord fourni, avec un classement des modèles selon leur complexité et leur utilisation prévue en phase pre-opératoire ou intra-opératoire. Nous proposons ensuite une première contribution sur la modélisation 3D d'une aiguille à pointe biseautée, qui consiste en deux modèles numériques pouvant être utilisés pour des applications en temps-réel et offrant la possibilité de considérer le cas de tissus en mouvement. Les performances des deux modèles sont évaluées et comparées à partir de données expérimentales.

Chapitre 3: Nous traitons le problème du suivi du corps d'une aiguille incurvée dans des volumes échographiques 3D. Les principes généraux de l'acquisition d'images échographiques sont tout d'abord décrits. Ensuite nous présentons une vue d'ensemble des algorithmes récents de détection et de suivi utilisés pour la localisation du corps de l'aiguille ou seulement de sa pointe dans des séquences images échographiques 2D ou 3D. Nous proposons ensuite une nouvelle contribution au suivi 3D d'une aiguille en exploitant les artefacts naturels apparaissant autour de l'aiguille dans des volumes 3D. Finalement nous proposons également une méthode de mise à jour de notre modèle d'aiguille en utilisant les mesures acquises pendant l'insertion pour prendre en compte les mouvements latéraux des tissus. Le modèle mis à jour est utilisé pour prédire la nouvelle position de l'aiguille et améliorer le suivi de l'aiguille dans le prochain volume 3D acquis.

Chapitre 4: Nous nous concentrons sur le sujet principal de cette thèse qui est le contrôle robotisé d'une aiguille flexible insérée dans des tissus mous sous guidage visuel. Nous dressons tout d'abord un état de l'art sur le guidage d'aiguilles flexibles, depuis le contrôle bas niveau de la trajectoire de l'aiguille jusqu'à la planification de cette trajectoire. Nous présentons ensuite la contribution principale de cette thèse, qui consiste en une approche de contrôle pour le guidage d'aiguille qui a la particularité d'utiliser plusieurs stratégies de guidage et qui est indépendante du type de manipulateur robotique utilisé pour actionner l'aiguille. Les performances de cette approche de vi Chapter 1

Introduction

Minimally invasive procedures have greatly expanded over the past century.

The traditional way to cure a patient has long been to resort to open surgery, which consists in making a large cut in the body to observe and manipulate its intern parts. The success rate of such an approach is first limited by the heavy modifications made to the body, which take time to heal and can lead to complications after the surgery. There is also a greater risk of subsequent infections due to the large exposure of the inner body to the outside environment. On the contrary, minimally invasive procedures only require a limited number of small incisions to access the organs. Therefore, this improves the overall well-being of the patient thanks to reduced postoperative pain and scarring. The recovery time of the patient is also greatly reduced [EGH + 13] along with the risk of infections [START_REF] Gandaglia | Effect of minimally invasive surgery on the risk for surgical site infections: Results from the national surgical quality improvement program (nsqip) database[END_REF], resulting in higher success rates for the operations and a cost reduction for the hospitals. When open surgery was necessary before the introduction of medical imaging, diagnosis and treatment could be two parts of a same intervention, in order to first see the organs and then plan and perform the required surgery. X-rays were among the first tools discovered to provide an anatomical view of the inside of the body without needing to open it. Several imaging modalities have since been developed and improved for medical purposes, among which computerized tomography (CT) [START_REF] Hounsfield | Computerized transverse axial scanning (tomography): Part 1. description of system[END_REF], magnetic resonance imaging (MRI) [START_REF] Lauterbur | Image formation by induced local interactions: Examples employing nuclear magnetic resonance[END_REF] and ultrasound (US) [START_REF] Wild | Application of echo-ranging techniques to the determination of structure of biological tissues[END_REF] are now widely used in the medical domain.

Beyond the improved diagnosis capabilities that it offers, medical imaging has played an important role in the development of the minimally invasive surgery approach. Viewing the inside of the body is necessary to perform successful surgical interventions, in order to see the tissues of interest and the position of the surgical tools. Due to the nature of minimally invasive surgery, a direct view is not possible and it is thus necessary to use other means of visual observation, such as endoscope insertion or anatomical imaging techniques. Each technique has its own advantages and drawbacks. Endoscopes use cameras, which offer the same view as a human eye. The 1 CHAPTER 1. INTRODUCTION images are thus easy to interpret, however it is not possible to see through the tissues. On the other hand, anatomical imaging allows a visualization of the inside of the tissues, but a specific training of the physicians is required in order to interpret the images. CT imaging uses X-rays, which are ionizing radiations, therefore limiting the number of images that can be acquired in order not to expose the patient to a too high amount of radiations [SBA + 09]. The medical staff should also remain outside the scanner room during the acquisition. On the other hand MRI makes use of non-invasive radiations and also provides high quality images, with high resolution and large field of view. However they impose severe constraints, such as a long time to acquire an image or an expensive and bulky scanner that limits the access to the patient. In this context, ultrasonography is a modality of choice for intra-operative imaging, due to its ability to provide a real-time visualization of tissues and tools in motion. Additionally, it is non-invasive and requires lightweight scanners and portable probes.

Slender tools are often used for minimally invasive procedures in order to be inserted through narrow incisions made at the surface of the patient. In particular, needles have been used since long times to extract or inject substances directly inside the body. They provide an access to inner structures while leaving only a very light wound in the tissues. For this reason they are tools of first choice for minimal invasiveness that can allow reaching small structures in deep regions. Thin needles can however exhibit a certain amount of flexibility, which makes accurate steering of the needle trajectory more complicated. Coupled to the handling of an US probe at the same time as the needle insertion gesture, the insertion procedure can become a challenging task which requires specific training of the clinician. Consequently, robotic needle steering has become a vast subject of research to ease the intervention of the clinician and to improve the overall accuracy of the procedure.

Medical robotics in general aims at designing and controlling mechatronics systems to assist the clinicians in their tasks. The main goal is to improve the accuracy, safety and repeatability of the operations and to reduce their duration [START_REF] Taylor | Medical robotics in computer-integrated surgery[END_REF]. It can greatly benefit the needle insertion procedures for which accuracy is often crucial to avoid mistargeting and unnecessary repeated insertions. However, the integration of a robotic system in the operating room remains a great challenge due to clinical constraints and acceptance of the technical device from the medical staff. Among the many designs that have been proposed, some systems have better chances of being accepted. Such systems should either assist the surgeon without requiring a lot of modifications of the clinical workflow or should procure clear benefits for both the success of the operation and the operating conditions of the surgeon. This is for example the case of imaging enhancement and tremor cancellation systems, or of tele-operated systems. For needle insertions procedures, this would mainly consists in providing a real-time monitoring of the 1.1. CLINICAL MOTIVATIONS state of the insertion as well as a robotic system between the patient and the hand of the surgeon assisting at the insertion process. In this context, an USguided robotic system is a great choice to provide real-time intra-operative imaging and assistance during the operation.

Clinical motivations

Needles are widely used in a great variety of medical acts for the injection of substances or the sampling of fluids or tissues directly inside the body. While some procedures do not require an accurate placement of the needle tip, such as intramuscular injections, the results of sensitive operations highly depend on the ability to reach a precise location inside the body. In the following we present some applications for which systematic accurate targeting is crucial to avoid dramatic consequences and which could greatly benefit from a robotic assistance.

Biopsy for cancer diagnosis

Cancer has become one of the major cause of death in the world with 8.2 million cancer deaths estimated worldwide in 2015 [TBS + 15]. Among the many types of cancers, prostate cancer is the most diagnosed cancer among men and breast cancer among women, with lung cancer being a leading cause of cancer deaths for both. However early detection of cancer can improve the chance of success of cancer treatment and diminish the mortality rates. Whatever the kind of tumor, biopsies are the traditional diagnostic method used to confirm the malignancy of suspected tissues. It consists in using a needle to get a small sample of tissues at a defined location for analysis purposes. The accurate placement of the needle is of paramount importance in this procedure to avoid misdiagnosis due to the sampling of healthy tissues surrounding the suspected lesion. Freehand insertions can give variable results depending on the clinician performing the operation. Therefore, robotic needle guidance under visual feedback has the potential to greatly improve the performances of biopsies. Ultrasound feedback is often used, as for example for the diagnostic of prostate cancer [START_REF] Kaye | Robotic ultrasound and needle guidance for prostate cancer manage-ment: review of the contemporary literature[END_REF]. Computerized tomography (CT) is also a good choice for lung cancer diagnosis and a robotic system is of great help to compensate for breathing motions [ZTK + 13]. Robotic systems can also be used to maintain and modify the position of the tissues to align a suspected tumor with the needle, especially for breast cancer biopsy [START_REF] Mallapragada | Robotassisted real-time tumor manipulation for breast biopsy[END_REF].

Brachytherapy

Brachytherapy has proven to be an efficient way to treat prostate cancer [GBS + 01]. It consists in placing small radioactive seeds in the tumors to CHAPTER 1. INTRODUCTION destroy. The procedure requires the accurate uniform placement of about a hundred seeds, which can be time consuming and require great accuracy. The consequence of misplacement can be dramatic due to the destruction of surrounding sensitive tissues like bladder, rectum, seminal vesicles or urethra. The insertion is usually performed under transrectal ultrasound, which can allow the use of robotic systems to perform accurate and repetitive insertions under ultrasound (US) guidance [START_REF] Hungr | A 3d ultrasound robotic prostate brachytherapy system with prostate motion tracking[END_REF] [SSK + 12] [START_REF] Kaye | Robotic ultrasound and needle guidance for prostate cancer manage-ment: review of the contemporary literature[END_REF]. Magnetic resonance imaging (MRI) is also commonly used and is the subject of research to explore its use together with a robotic system [START_REF] Seifabadi | Toward teleoperated needle steering under continuous mri guidance for prostate percutaneous interventions[END_REF].

Liver cancer

Liver cancer is the major cause of cancer deaths after lung cancer among men with about 500000 deaths each year [TBS + 15]. Radiofrequency ablation is the primary therapetic modality to perform liver tumor ablations [START_REF] Lencioni | Percutaneous image-guided radiofrequency ablation of liver tumors[END_REF]. An electrode needle is inserted in the liver and generates heat to locally destroy the tissues. Accurately guiding the needle under imageguidance can help avoiding unnecessary tissue destruction. Liver biopsies can also be performed using percutaneous punction needles [START_REF] Grant | Guidelines on the use of liver biopsy in clinical practice[END_REF]. Performing robotic ultrasound (US) guidance could avoid multiple insertions that increase hepatic bleeding and can have dramatic consequences.

Scientific context

Reaching a specific region in the body without performing open surgery is a challenging task that has been a vast subject of research and developments over the past decades. Many robotic designs have been proposed to achieve this goal. In the following we present a non-exhaustive overview of these different technologies as well as various kinds of sensor modalities that have been developed and used to provide feedback on the medical procedure. We then define where we positioned the work presented in this thesis relative to this context.

Robotic designs

Continuum robots: These systems are snake-like robots consisting of a succession of actively controllable articulations, as can be seen in Fig. 1.1a. They offer a large control over their whole shape and can be used to perform many kinds of operations. Many varieties of designs are possible and the study of such robots is a vast field of research by itself [START_REF] Walker | Robot strings: Long, thin continuum robots[END_REF] [START_REF] Burgner-Kahrs | Continuum robots for medical applications: A survey[END_REF]. However their design and control are often complex and their diameter is usually larger than standard needles, which limit the use of such system in practice. Concentric tubes: This kind of robots, also known as active cannulas, is a special kind of continuum robots which consist of a telescopic set of flexible concentric pre-curved tubes that can slide and rotate with respect to each other [START_REF] Webster | Design and kinematic modeling of constant curvature continuum robots: A review[END_REF]. Each tube is initially maintained inside the larger tubes and the insertion of such device is performed by successively inserting each set of tubes and leaving in place the outermost tubes one after another, as seen in Fig. 1.1b. They offer additional steering capabilities compared to flexible needles due to the pre-curved nature of each element, while maintaining a relatively small diameter. Furthermore, once the tubes have been deployed, rotation of the different elements allows for controlled deformations of the system all along its body. Although the design can be limited to only one pre-curved stylet placed in an outer straight tube, as was proposed in [OEC + 05], some other designs are possible to enable an additional control of the curvature of each tube [START_REF] Chikhaoui | Kinematics and performance analysis of a novel concentric tube robotic structure with embedded soft micro-actuation[END_REF]. As continuum robots, the modeling and control of such systems remain quite complex [START_REF] Dupont | Design and control of concentric-tube robots[END_REF] [BLH + 16].

SCIENTIFIC CONTEXT

Needle insertion devices: Many robotic systems have been designed for the insertion of traditional needles and particularly for asymmetric tip needles. Several kinds of asymmetries are possible, as illustrated in Fig. 1.2. These needles tend to naturally deviate from a straight trajectory, such that the rotation around their shaft plays an important role. Many needle Figure 1.3: General concept of a needle insertion device (taken from [START_REF] Webster | Design considerations for robotic needle steering[END_REF]). insertion systems have been proposed, all being a variant of the same design consisting of one linear stage for the insertion and one rotative stage for needle rotation along and around its main axis [START_REF] Webster | Design considerations for robotic needle steering[END_REF], as depicted in Active needles: Alternatively, many designs have been proposed to replace traditional needles and provide additional control capabilities over their bending. A needle made of multiple segments that can slide along each other was designed such that the shape of the tip can be modified during the insertion [START_REF] Ko | Closedloop planar motion control of a steerable probe with a "programmable bevel"; inspired by nature[END_REF]. A 1 degree of freedom (DOF) actuated needle tip was designed such that it can act as a pre-bent tip needle with variable angle between the shaft and the pre-bent tip [AGL + 16]. A similar tendon-actuated needle tip with 2 DOF was also used to allow the orientation of the tip without rotation of the needle around its axis [START_REF] Roesthuis | Modeling and steering of a novel actuatedtip needle through a soft-tissue simulant using fiber bragg grating sensors[END_REF]. Additional considerations about tip designs can be found in [START_REF] Van De Berg | Design choices in needle steering-a review[END_REF]. These needle designs allows a high controllability of the tip trajectory, however, in addition to the increased complexity of the needle itself, they require a special system to be able to control the additional DOF from the needle base.

A combination of different methods can also be made as was done in [SMR + 15], where using a succession of cable driven continuum robot, concentric tubes and beveled-tip needle increases the reachable space and final accuracy of the targeting.

Sensor feedback

In order to be used for needle insertion assistance, a robotic system should be able to monitor the state of the insertion. Therefore, feedback modalities have to be used to provide some information on the needle and the tissues. The choice of the sensors is an important issue that has to be taken into account from the beginning of the conception of the system. Indeed, they should either be directly integrated into the system or they can pose compatibility issues in the case of external modalities. In the following we provide an overview of some feedback modalities currently used or explored for needle insertion procedures.

Shape feedback: The shape of the entire needle can be reconstructed using fiber Bragg grating (FBG) sensors. This kind of sensor consists in several optic fibers integrated in the needle. The light propagates differently in these fibers depending on the curvature of the fiber at certain locations, such that the curvature of the needle can be measured and used to retrieve its shape [PED + 10]. This kind of sensor requires a special design of the needle, since the fibers need to follow the same deformations as the needle does.

An electromagnetic (EM) tracker can also be used for the tracking of the position and orientation of a specific point of the needle, which is typically the tip. They provide a great accuracy on the measures and currently available trackers are small enough such that they can fit directly in standard needles.

Real-time imaging modalities: Feedback on the needle position is not sufficient for needle insertions since the position of the targeted region must also be known. Using an imaging modality can provide a visual feedback on the position of both the needle and the target. Ultrasound (US) is the CHAPTER 1. INTRODUCTION modality of choice for real-time imaging due its fast acquisition rate of 2D or 3D images, good resolution and safety. Special 2.5D US transducers are also the subject of current research to enable a direct detection and display of the needle tip in a 2D US image, even when the tip is outside the imaging plane of the probe [XWF + 17]. However, these transducers are currently not commonly available. A limiting factor of US in general is the low quality of the images due to the intrinsic properties of US waves.

On the other hand, computerized tomography (CT)-scan or magnetic resonance imaging (MRI) are used for manual insertions thanks to the high quality of their images and the large field of view that they offer. However, as stated previously, this kind of imaging method can not be used directly for real-time image-guided robotic needle insertion, due to their long acquisition time. They can still be used for non real-time tele-operated robotic control, by alternating between insertion steps and imaging steps, however a single needle insertion can take more than 45 minutes. Tissue motions between two acquisitions is also an issue that requires additional real-time sensors to be compensated for, such as force sensors [MDG + 05] or optical tracking [ZTK + 13].

On the contrary CT fluoroscopy can be used to acquire real-time images. However, in manual needle insertion this exposes the clinician to a high dose of noxious radiations. This can be avoided by wearing unpractical special shielding or by using a remotely controlled insertion system [SPB + 02]. However the patient is still exposed to the high amount of radiations necessary for real-time performances.

Fast MRI acquisition has also recently been explored to perform imageguided needle insertion [PvKL + 15]. Decreasing the image size and quality, a 2D image could be acquired with a sufficient resolution every 750 ms. By comparison, the US modality can provide a full 3D volume with similar resolution within the same acquisition time, and 2D US can acquire several tens of images per second.

Force feedback: Force sensors can be used to measure the forces applied to the needle and tissues. Force sensing can be useful to monitor the state of the insertion, for example by detecting the perforation of the different structures that the needle is going through [START_REF] Okamura | Force modeling for needle insertion into soft tissue[END_REF]. It can also be used with tele-operated robotic systems to provide a feedback to the clinician [PBB + 09] or compensate for tissue motion [START_REF] Joinié-Maurin | Force feedback teleoperation with periodical disturbance compensation[END_REF]. Any kind of force sensors can be used with the US modality, however compatibility issues have to be taken into account for the design of sensors compatible with CT [KPM + 14] or MRI [START_REF] Gassert | Sensors for applications in magnetic resonance environments[END_REF].

CHALLENGES

Objectives

The objective of this thesis is to focus on the robotic steering of traditional flexible needles. These needles are already widely available and used in clinical practice. Moreover they do not require specific hardware, contrary to other special robotic designs, which requires dedicated control hardware and techniques. The idea is then to provide a generic formulation of the different concepts that we introduce, such that our work can be adapted to several kinds of needle tip and rigidity. In this context, the control of the full motion of the needle base should thus be performed, such that it is not limited to flexible beveled-tip needles but can also be used to insert rigid symmetric tip needles. The formulation should also stay as much as possible independent of the actual robotic system used to perform the needle steering. This choice is motivated by the fact that it would ease the clinical acceptance of the method and could be applicable to several robotic systems and medical applications.

Another objective is to focus on the insertion under ultrasound (US) guidance, motivated by the fact that it is already used in current medical practice and does not require any modification of the needle to provide a real-time feedback on its whole shape. For the development and validation of our work, we try to keep in mind some clinical constraints related to the set-up and registration time, which should be as small as possible. Several other modalities have also to be explored, such as force feedback and electromagnetic (EM) feedback, which can easily be implemented alongside traditional needles and the US modality.

Challenges

In order to fulfill our objective of performing the ultrasound-guided control of a robotic system for the insertion of a flexible needle in soft tissues, several challenges needs to be addressed. We describe these different challenges in the following.

Interaction modeling: First, the control of the insertion of a flexible needle with a robotic system requires a model of the interaction between the needle and soft tissues. The effect of the inputs of the robotic insertion system on the needle position and effective length have to be modeled as well. The model should be complete to represent the whole body of the needle in 3D as well as the influence of the tip geometry on its trajectory. It should also be generic enough so that it can be easily adaptable to several kinds of needles. Since it is used for intra-operative purposes, it should be able to represent the current state of the insertion, taking into account the effect of potential motions of the tissues on the deformation of the needle. While complex and accurate models of the needle and tissues exist, the complexity of the modeling must remain reasonable such that real-time performances can be achieved.

Needle control:

The control of the trajectory of a flexible needle is a challenging task in itself. The complex interaction of the needle with the tissues at its tip and along its shaft is difficult to completely predict, especially because of the great variety of behaviors exhibited by biological tissues. Accurately reaching a target requires then to take into account and to exploit the flexibility of the needle and the motion of the tissues. The safety of the operation should also be ensured to avoid excessive damage caused by the needle onto the tissues. This is a difficult task since inserting the needle necessarily introduces a certain amount of tissue cutting, and steering the needle can only be achieved through an interaction of the needle with the tissues.

Tissue motion: Many needle insertion procedures are not performed under general anesthesia. As a consequence, physiological motions of the patient can not always be controlled. Patient motions can have several effects on the needle insertion. First it introduces a motion of the targeted region, which should be accounted for in order to avoid mistargeting. The trajectory of a flexible needle could also be modified by tissue motions. During manual needle insertion, clinicians can directly see the motion of the skin of the patient and feel the forces applied on the needle and tissues, such that they can easily follow the motions of the patient if needed. A robotic system should also be able to adapt to some motions of the patient while inserting the needle to avoid threatening the safety of the operation. This point represents a great challenge due to the limited perception available for the robotic system.

Needle detection: Accurate localization of the needle in ultrasound (US) images is a necessary condition to be able to control the state of the insertion. The low quality of US images is a first obstacle to the accurate localization of the needle tip. It can greatly vary depending on the tissues being observed and the position of the needle relatively to the US probe. Using 3D US has the advantage that the whole shaft of the needle can be contained in the field of view of the US probe, which is not the case with 2D US. However, even in 3D US the needle is not equally visible at all points due to specific artifacts that can come from the surrounding tissues or from the needle itself. Even if the 3D volume acquisition is relatively fast, the position of the needle in the volume can still greatly vary due to the motion of the patient or of the probe between two acquisitions. Overall needle localization using US feedback represents a challenging task that is still an open issue that has to be addressed.

CONTRIBUTIONS

Contributions

In order to address the challenges mentioned previously, we present several contributions in this thesis, which are:

• two 3D models of the interaction between a flexible needle with a bevel tip and soft tissues. The models are designed to allow real-time processing and to provide a 3D representation of the entire needle body during the insertion in moving tissues;

• a method to estimate the lateral motions of the tissues using only the measures available on the needle;

• a method for tracking a flexible needle in 3D ultrasound volumes taking into account the artifacts inherent to the ultrasound modality;

• the design of a framework for the control of a robotic system holding a flexible needle inserted in soft tissues. The framework is designed to be easily adaptable to any hardware components, whatever the needle type, the robotic system used for the control of the needle motion or the feedback modality used to provide information on the needle location. It can also provide hybrid control strategies like manipulation of the lateral motions of the needle base or tip-based steering of the needle tip;

• the ex-vivo validations of the proposed methods using various experimental platforms and scenarios in order to illustrate the flexibility of the framework in performing needle insertions.

The contributions on the topic of an hybrid control strategy used to steer a flexible needle under visual feedback were published in an article in the proceedings of the International Conference on Robotics and Automation (ICRA) [START_REF] Chevrie | Needle steering fusing direct base manipulation and tip-based control[END_REF]. The contributions on the topic of needle modeling and tissue motion estimation using visual feedback were published in an article in the proceedings of the International Conference on Intelligent Robots and Systems (IROS) [START_REF] Chevrie | Online prediction of needle shape deformation in moving soft tissues from visual feedback[END_REF].

Experimental context

Experiments presented in this thesis were primarily conducted on the robotic platform of the Lagadic team at IRISA/Inria Rennes, France. Others were also conducted at the Surgical Robotics Laboratory attached to the University of Twente, Enschede, the Netherlands. This offered the opportunity to test the genericity of our methods using different experimental setups. We present in this section the list of the different equipments that we used in the different experiments presented all along this thesis.

CHAPTER 1. INTRODUCTION

The general setup that we used is made up of four parts: a needle attached to a robotic manipulator, several homemade phantoms simulating soft tissues, a set of sensors providing various kinds of feedbacks and a workstation used to process the data and manage the communications between the different components.

Robots

Two different kinds of needle manipulation systems were used, the first one in France and the second one in the Netherlands.

• The Viper s650 and Viper s850 from Omron Adept Technologies, Inc.

(Pleasanton, California, United States) are 6 axis industrial manipulators, depicted in Fig. 1.5a. The robots communicate with the workstation through a FireWire (IEEE 1394) connection. They were used to hold and actuate the needle or to hold the 3D ultrasound (US) probe. They were also used to apply motions to the phantom in order to simulate patient motions.

• The UR3 and UR5 from Universal Robots A/S (Odense, Denmark) are 6 axis table-top robots, depicted in Fig. 1.5b. Both robots were connected to a secondary workstation and communicated through Ethernet using Robot Operating System (ROS) (Open Source Robotics Foundation, Mountain View, USA). UR3 was used to hold and actuate an insertion device described in the following. UR5 is a larger version of UR3 and was used to apply a motion to the phantom.

We also used a 2 degrees of freedom needle insertion device (NID), visible in Fig. 1.5b, designed at the Surgical Robotics Laboratory [START_REF] Shahriari | Design and evaluation of a computed tomography (CT)compatible needle insertion device using an electromagnetic tracking system and CT images[END_REF], which controls the insertion and rotation of the needle along and around its axis. A Raspberry Pi 2 B (Raspberry Pi foundation, Caldecote, United Kingdom) along with a Gertbot motor controller board (Fen logic limited, Cambridge, United Kingdom) were used to control the robot through pulse-width-modulation (PWM).

Motor encoders were used to measure the position and rotation of the needle, allowing to know its effective length that can bend outside the NID. The NID was connected to the end effector of the UR3 through a plastic link, as can be seen in Fig. 1.5b, allowing the control of the 3D pose of the NID with the UR3.

Visual feedback systems

We used two different modalities to provide a visual feedback on the needle and phantom position. Cameras were used for the evaluation of the performances of the control framework and ultrasound (US) probes were used to validate the framework using a clinical modality.

We used in France two Point Grey FL2-03S2C cameras from FLIR Integrated Imaging Solutions Inc. (formerly Point Grey Research, Richmond, BC, Canada), which are color cameras providing 648 x 488 images with a frame rate up to 80 images per second. Each camera was coupled with a DF6HA-1B lens from Fujifilm (Tokyo, Japon), which has a 6 mm focal length with manual focus. The cameras send the acquired images to the workstation through a FireWire (IEEE 1394) connection. This system was used only with translucent gelatin phantoms to enable the observation of the needle for validation purposes. Both cameras and a gelatin phantom can be seen in Fig. 1.6. A white screen monitor or a piece of paper were used to offer a uniform background behind the phantom that facilitates the segmentation of the needle in the images.

Two different US systems were used for the experiments. For the experiments performed in France, we used a 4DC7-3/40 convex 4D US probe (see Fig. 1.7a) from BK Ultrasound (previously Ultrasonix Medical Corporation, Canada), which is a wobbling probe with frequency range from 3 MHz to 7 MHz, transducer radius of 39.8 mm and motor radius of 27.25 mm. This probe was used with a SonixTOUCH research US scanner from BK Ultrasound (see Fig. 1.7b). The station allows an access to raw data via an Ethernet connection, such as radio frequency data or pre-scan B-mode data.

For the experiments performed in the Netherlands, we used a 7CF2 Convex Volume 4D/3D probe (see Fig. 1.7c) from Siemens AG (Erlangen, Germany), which is a wobbling probe with frequency range from 2 MHz to 7 Mhz, transducer radius of 44.86 mm and motor radius of 14.84 mm. This probe was used with an Acuson S2000 US scanner from Siemens (see Fig. 1.7d). This station does not give access to raw data nor online access to transformed data. Pre-scan 3D US volumes can be retrieved offline using the digital imaging and communications in medicine (DICOM) format. Nevertheless, 2D images were acquired online using a USB frame grabber device from Epiphan Video (Ottawa, Ontario, Canada) connected to the video output of the station.

Phantoms

Different phantoms were used for the experiments. Porcine gelatin was used in all phantoms, either alone or while embedding ex-vivo biological tissues. We used either porcine or bovine liver as biological tissues. The gelatin and tissues were embedded in transparent plastic containers of different sizes. Various artificial targets were also embedded in some phantoms, in the form of raisins or play-dough spheres of different sizes, ranging from 4 mm to 8 mm.

Workstations

All software developments were made using the C++ language. We used the ViSP library [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] as a basis for the majority of the control framework, image processing, graphics user interface and communications. CUDA library was used for optimization of the post-scan conversion of 3D ultrasound volumes with a Nvidia GPU. Eigen library was used for fast matrix inversion for the needle modeling.

For the experiments performed in France we used a workstation running Ubuntu 14.04 LTS 64-bit and equipped with 32 GB memory, a Intel R Xeon R E5-2620 v2 @2.10GHz × 6 CPU and a NVIDIA R Quadro R K2000 GPU.

For the experiments performed in the Netherlands we used a personal computer running Fedora 24 64-bit and equipped with 16GB memory and a Intel R Core TM i7-4600U @2.10 Ghz × 4 CPU.

Needles

We summarize the characteristics of the different needles used in the experiments in Table 1.1. A picture of the needle used in France and a zoom on the beveled tip can be seen in Fig. 1.8.

Force sensor

For the experiments performed in the Netherlands, we used a Nano 43 force torque sensor from ATI Industrial Automation (Apex, USA), which is a sixaxis sensor measuring forces and torques in all 3 Cartesian directions with a resolution of 1.95 mN for forces and 25 µN.m for torques. The sensor was placed between the UR3 robot and the needle insertion device to measure the interaction efforts exerted at the base of the needle, as depicted in Fig. 1.9a.

Electromagnetic tracker

For the experiments performed in the Netherlands, we used an Aurora v3 electromagnetic (EM) tracking system from Northern Digital Inc. (Waterloo, Canada), which consists in a 5 degrees of freedom EM sensor (see Fig. 1.9b) placed in the tip of the needle and an EM field generator (see Fig. 1.9c).

The system is used to measure the 3D position and axis alignment of the needle tip, with an position accuracy of 0.7 mm and an orientation accuracy of 0.20 • , at a maximum rate of 65 measures per second.

Thesis outline

In this chapter we presented the clinical and scientific context of this thesis. We defined our general objective as being the robotic insertion of a flexible needle in soft tissues under ultrasound (US) guidance and we described the associated challenges. A list of the equipments used in the various experiments presented in this thesis was also provided. The remaining of this manuscript is organized as follows.

Chapter 2: We present an overview of needle-tissue interaction models. A review of different families of models is first provided, with a classification of the models depending on their complexity and intended use for pre-operative or intra-operative purposes. We then propose a first contribution on the 3D modeling of a beveled-tip needle interacting with soft tissues consisting of two numerical models that can be used for real-time applications and offering the possibility to consider the case of moving tissues. The performances of both models are evaluated and compared through experiments.

Chapter 3: We address the issue of tracking the body of a curved needle in 3D US volumes. The general principles of the acquisition process of US images and volumes are first described. Then we present an overview of recent detection and tracking algorithms used to localize the whole needle body or only the needle tip in 2D or 3D US sequences. We then propose a new contribution to 3D needle tracking that exploits the natural artifacts appearing around the needle in US volumes. Finally we also propose a method to update our needle model using the measures acquired during the insertion to take into account lateral tissue motions. The updated model is used to predict the new position of the needle and to improve needle tracking in the next acquired US volume.

Chapter 4: We focus on the core topic of this thesis which is the robotic steering of a flexible needle in soft tissues under visual guidance. We first provide a review of current work on flexible needle steering, from the low level control of the needle trajectory to the planning of this trajectory. We then present the main contribution of this thesis, which consists in a needle steering framework that has the particularity to include several steering strategies and which is independent of the robotic manipulator used to steer the needle. The performances of the framework are illustrated through several ex-vivo experimental scenarios using cameras and 3D US probes as visual feedback.

Chapter 5: We consider the issue of patient motions during the needle insertion procedure. An overview of motion compensation techniques during needle insertion is first presented. We further extend our steering framework proposed in chapter 4 and we exploit the model update method proposed in chapter 3 in order to handle needle steering under lateral motions of the tissues. We provide experimental results obtained by using the proposed framework to perform needle insertion in a moving soft tissue phantom. These experiments were performed using several information feedback modalities, such as a force sensor, an electromagnetic tracker as well as 2D US.

Conclusion:

Finally we provide the conclusion of this dissertation and present perspectives for possible extensions and applications.

Chapter 2

Needle insertion modeling

This chapter provides an overview of needle-tissue interaction models. The modeling of the behavior of a needle interacting with soft tissues is useful for many aspects of needle insertion procedures. First it can be used to predict the trajectory of the needle tip, before inserting the real needle. This can be of great help to the clinicians in order to find an adequate insertion entry point that optimizes the chances of reaching a targeted region inside the body, while reducing the risks of damaging other sensitive regions. Secondly, using thinner needles allows decreasing the patient pain and the risk of bleeding [START_REF] Gill | Does needle size matter?[END_REF]. However, the stiffness of a thin needle is greatly reduced and causes its shaft to bend during the insertion. This makes the interaction between the needle and tissues more complex to comprehend by the clinicians, since the position of the needle tip is not directly known from the position and orientation of the base, contrary to rigid needles. The introduction of a robotic manipulator holding the needle and controlling its trajectory can be of great help to unburden the operator of the needle manipulation task. This removes a potential source of human error and leaves the clinicians free to focus on other aspects of the procedure [START_REF] Abolhassani | Needle insertion into soft tissue: A survey[END_REF]. Needle-tissue interaction models are a necessity for the usage of such robotic systems, in order to know how they should be controlled to modify the needle trajectory in the desired way.

In the following, we first provide a review of needle-tissue interaction models. We address the case of kinematic models (section 2.1), which only consider the trajectory of the tip of the needle, and the case of finite element modeling (section 2.2) that can completely model the behavior of the needle and the surrounding tissues. Then we present mechanics-based models (section 2.3) used to represent the body of the needle without modeling all the surrounding tissues. We further extend on this topic and propose two new 3D models of a needle locally interacting with soft tissues (section 2.4). Finally, in section 2.5 we compare the trajectories of the needle tip obtained with both models to the trajectories obtained during the insertion of a real CHAPTER 2. NEEDLE INSERTION MODELING needle. The work done using both models was published in two articles presented in international conferences [CKB16a] [START_REF] Chevrie | Online prediction of needle shape deformation in moving soft tissues from visual feedback[END_REF].

Kinematic modeling

During the insertion of a needle, a force is applied to the tissues by the needle tip to cut a path in the direction of the insertion. In return the tissues apply reaction forces to the needle tip and the direction of this forces depends on the geometry of the tip, as illustrated in Fig. 2.1.

In the case of a symmetric needle tip, the lateral forces tends to negate each other, leaving only a force aligned with the needle. The needle tip trajectory then follows a straight line when the needle in inserted. However when the needle tip has an asymmetric shape, as for example in the case of a beveled or pre-curved tip, inserting the needle results in a lateral reaction force. The needle trajectory bends in the direction of the reaction force. The exact shape of the trajectory depends on the properties of the needle and tissues. The stiffness of the needle introduces internal forces that naturally act against the bending of the shaft. The deformations of the tissues also creates forces all along the needle body, which modify its whole shape.

Kinematic modeling is used under the assumption that the tissues are stationary and no lateral motion is applied to the needle base, such that the different forces are directly related to the amount of deflection observed at the tip. The value of all these forces are ignored in this case and only the trajectory of the tip is represented from a geometric point of view. The whole needle shaft is ignored as well and the insertion and rotation along and around the needle axis are assumed to be directly transmitted to the tip. This way the modeling is limited to the motion of the tip during the insertion or rotation of the needle. Note that this kind of representation is limited to asymmetric geometries of the tip, since a symmetric tip would only produce a straight trajectory that does not require a particular modeling.

Kinematic modeling of the behavior of a needle during its insertion was Unicycle model: The 2D unicycle consists in modeling the tip as the center of a single wheel that can translate in one direction and rotate along another normal direction, as illustrated in Fig. 2.2a. During the needle insertion, the needle tip is assumed to follow a circular trajectory. The ratio between the translation and rotation is fixed by the natural curvature K nat of this circular trajectory and depends on the needle and tissue properties such that

   ẋ = v ins cos(θ) ẏ = v ins sin(θ) θ = K nat v ins , (2.1)
where x and y are the coordinates of the wheel center, i.e. the needle tip, θ is the orientation of the wheel and v ins is the insertion velocity.

Bicycle bicycle:

The 2D bicycle model uses two rigidly fixed wheels at a distance L w from each other, such that the front wheel lies on the axis of the rear wheel and is misaligned by an fixed angle φ, as illustrated in Fig. 2.2b. The point representing the needle tip lies somewhere between the two wheels, at a distance L t from the rear wheel. In addition to the rotation and the velocity in the insertion direction observed with the unicycle model, the tip is also subject to a lateral translation velocity, directly linked to the CHAPTER 2. NEEDLE INSERTION MODELING distance L t . The trajectory of the tip is then described according to

       ẋ = v ins cos(θ) -Lt Lw tan(φ) sin(θ) ẏ = v ins sin(θ) + Lt Lw tan(φ) cos(θ) θ = tan(φ) Lw v ins , (2.2)
where x and y are the coordinates of the needle tip, θ is the orientation of the rear wheel and v ins is the insertion velocity. This model is equivalent to the unicycle model when the tip is at the center of the rear wheel, i.e. L t = 0.

Rotation around the needle axis: The rotation around the needle axis is also taken into account in kinematic models. They were first mainly used in the 2D case, such that the needle tip stays in a plane [RMK + 11]. The tip can then only describe a curvature toward the right or the left, such that a change of direction corresponds to a 180 • rotation of a real 3D needle. The tip trajectory is thus a continuous curve made up of a succession of arcs. However, a better modeling of the needle insertion is achieved by considering the 3D case, where the rotation along the needle axis is continuous. In this case the orientation of the asymmetry fixes the direction in which the tip trajectory describes a curve during the needle insertion. This can lead to a greater variety of motions, such as helical trajectories [HAC + 09].

Discussion: Kinematic modeling is easy to implement since it needs few parameters and is not computationally expensive. However the relationship between the model parameters and the real needle behavior is difficult to model since they depend on the needle geometry and tissue properties. In practice these parameters are often identified after performing some preliminary insertions in the tissues. Since this is not feasible in practice for real surgical procedures, online estimation of the natural curvature of the tip trajectory can be performed, for example by using a method based on a Kalman filter as proposed by Moreira et al. [START_REF] Moreira | Needle steering in biological tissue using ultrasound-based online curvature estimation[END_REF]. It can also be observed that the trajectories obtained with both unicycle and bicycle models are limited. For example they are continuous when a rotation without insertion is performed between two insertion steps. The two successive parts of the trajectory are tangent if the unicycle model is used and are not tangent if the bicycle model is used. However, both models fail to describe the trajectory of a pre-bent needle, for which a translational offset is also added when the needle is rotated. Hence modifications have to be made to account for the fact that the tip is not aligned with the axis of the rotation [RKA + 08].

Another point is that kinematic models do not take into account the interaction between the body of the needle and the tissues. The shaft is 2.2. FINITE ELEMENT MODELING assumed to exactly follow the trajectory of the needle tip and has no influence on this trajectory. This assumption can only hold if the needle is very flexible and the tissues are stiff, such that the forces due to the bending of the needle are small enough to cause very little motion of the tissues. The tissues must also be static, such that they do not modify the position of the needle during the insertion. These assumptions are easy to maintain during experimental research work, but harder to maintain in clinical practice due to patient physiological motions and variable tissue stiffness. An extension of the bicycle model that takes into account additional lateral translations of the needle tip is possible [FKR + 15]. This allows a better modeling of the tip motion, but it requires additional parameters that need to be estimated and can vary depending on the properties of the tissues, which limits its practical use.

Finite element modeling

Finite element modeling (FEM) is used to model the whole tissue and needle. In addition to the effect of the needle-tissue interaction on the needle shape, the resulting deformations of the tissues are also computed. The method consists in using a finite set of elements interacting with each other, each element representing a small region of interest of the objects being modeled, as can be seen in Fig. 2.3a. This allows the modeling of the needle deformations as well as the motion of a targeted region in the tissues, due to the needle interaction or due to external manipulation of the tissues [THA + 09][PVdBA11]. This requires a description of the geometry of the tissues and the needle as well as a certain amount of physical parameters for all of them that depends on the chosen complexity of the mechanical model.

In general the computational complexity of such models is high when compared to other modeling methods. The time required for the computations increases with the level of details of the model, i.e. the number

CHAPTER 2. NEEDLE INSERTION MODELING

of elements used to represent each object, and the number and complexity of the phenomena taken into consideration. Modeling the exact boundary conditions and properties of real in vivo objects, such as different organs in the body, is also a challenging task. This makes FEM hard to use for real-time processing without dedicated hardware optimization and limits its use to pre-planning of needle and target trajectories [AGPH09] [START_REF] Hamzé | Preoperative trajectory planning for percutaneous procedures in deformable environments[END_REF]. However, it offers a great flexibility on the level of complexity, which can be chosen independently for the different components in the model. We provide in the following a short overview of different models that can be used for the needle and the tissues.

Needle: Various complexity can be chosen for the needle model. A 1D beam model is often used under various forms. It can for example be a rigid beam [START_REF] Dimaio | Needle insertion modeling and simulation[END_REF], a flexible beam [START_REF] Dimaio | Interactive simulation of needle insertion models[END_REF] or a succession of rigid beams linked by angular springs [GDS09] [START_REF] Haddadi | Development of a dynamic model for bevel-tip flexible needle insertion into soft tissues[END_REF]. The needle geometry can also be modeled entirely in 3D to accurately represent its deformations and the effect of the tip geometry [MRD + 08][YTS + 14].

Tissues: Tissues can also be modeled with different levels of complexity, ranging from a 2D rectangular mesh with elastostatic behavior [START_REF] Dimaio | Interactive simulation of needle insertion models[END_REF] to 3D mesh with real organ shape [CAR + 09] (see Fig. 2.3b) and dynamic nonlinear behavior [START_REF] Tang | Constraint-based soft tissue simulation for virtual surgical training[END_REF].

The complexity of the interactions between the needle and the tissues can also vary. In addition to interaction forces due to the lateral displacements of the needle, tangential forces are often added as an alternation between friction and stiction along the needle shaft [DGM + 09], introducing a highly non-linear behavior. The complexity of the tissue cutting at the needle tip and along the needle shaft can also greatly vary. It usually involves a change in the topology of the model [CAK + 14], which can be simple to handle if the needle is modeled as a 1D beam [START_REF] Goksel | Haptic simulator for prostate brachytherapy with simulated needle and probe interaction[END_REF] or more complex when using a 3D modeling of the needle and non-linear fracture phenomenon in the tissues [START_REF] Oldfield | Detailed finite element modelling of deep needle insertions into a soft tissue phantom using a cohesive approach[END_REF][YTS + 14].

Mechanics-based modeling

Mechanics-based models are used to model the entire needle shaft of the needle and its interactions with the surrounding tissues. The needle is thus often modeled as a 1D beam with a given flexibility that depends on the mechanical properties of the real needle. On the other hand, the tissues are not entirely modeled as is done with finite element modeling (FEM) but only the local interaction with the needle is taken into account.

MECHANICS-BASED MODELING

Bernoulli beam equations:

A first way to model the interaction between the needle shaft and the tissues is to use a set of discrete virtual springs placed along the shaft of the needle, as was done in 2D by Glozman et al. [START_REF] Glozman | Image-guided robotic flexible needle steering[END_REF].

The needle is cut into multiple flexible beams and virtual springs are placed normal to the needle at the intersection of the beam extremities, as depicted in Fig. 2.4a. Knowing the position and orientation of the needle base and the position of the springs, the shape of the needle can be computed using the Bernoulli beam equations for small deflections. Concerning the interaction of an asymmetric needle tip with the tissues, a combination of axial and normal virtual springs can also be used to locally model the deflection of the tip [START_REF] Dorileo | Needle deflection prediction using adaptive slope model[END_REF].

Instead of using discrete springs, the Bernoulli equations can also be applied when the needle-tissue interaction is modeled using a distributed load applied along the needle shaft [KFR + 15], as illustrated in Fig. 2.4b. This allows a continuous modeling of the interaction along the needle shaft, resulting in a smoother behavior compared to the successive addition of discrete springs.

CHAPTER 2. NEEDLE INSERTION MODELING

Energy-based method: An energy-based variational method can also be used, instead of directly using the Bernoulli equations to solve the needle shape. This method, known as the Rayleigh-Ritz method and used by Misra et al. [MRS + 10], consists in computing the shape of the needle that minimizes the total energy stored in the system. It has been shown that this energy is mainly the sum of the bending energy stored in the needle, the deformation energy stored in the tissues and the worksthat are due to tissue cutting at the tip and the insertion force at the base. This method can be combined with different models of the interaction of the needle with the tissues, as long as a deformation energy can be computed. For example, a combination of virtual springs along the needle shaft and continuous load at the end of the needle can be used [START_REF] Roesthuis | Mechanicsbased model for predicting in-plane needle deflection with multiple bends[END_REF]. Different methods are also available to define these continuous loads. They can be computed depending on the distance between the needle and the tissues, as a continuous version of the virtual springs. In this case the position of the tissues can be taken depending on a previous position of the needle shaft [MRS + 10] or tip [KFR + 15]. The continuous load can also directly be estimated online [START_REF] Wang | Mechanics-based modeling of needle insertion into soft tissue[END_REF].

The two methods stated above can also be used along with pseudocontinuous models of the needle instead of the continuous one. In this case the needle is modeled using a succession of rigid rods linked by angular springs which are used to model the compliance of the needle [START_REF] Goksel | Modeling and simulation of flexible needles[END_REF]. Such a model is more simple since the parameters to describe its shape only consist in the angles observed between successive rods, without requiring additional parameters for the shape of these rods.

Dynamic behavior:

The different models presented in this section mainly allow modeling the quasi-static behavior of a needle inserted in the tissues. The dynamics of the insertion can also be modeled by adding a mass to the needle beams, some visco-elastic properties to the elements modeling the tissues and a model of friction along the shaft [YPY + 09][KRU + 16]. The friction mainly occurs during the insertion of the needle, nevertheless it has been shown that a rotation lag between the needle base and the needle tip could also appear [START_REF] Reed | Modeling and control of needles with torsional friction[END_REF]. Hence a model of torsionnal friction and needle torsion can be added when the needle rotates around its axis [START_REF] Swensen | Torsional dynamics of steerable needles: Modeling and fluoroscopic guidance[END_REF]. However, as stated in previous section 2.1 for kinematic models, each additional layer of modeling requires the knowledge or estimation of new parameters, in addition to the increased computational complexity. Hence, the number of modeled phenomena that can be included depends on the intended use of the model: a high number for offline computations, hence approaching FEM models, or a reduced number to keep real-time capabilities, like kinematic models.

Generic model of flexible needle

In this section we describe and compare two models that we propose for the 3D modeling of a flexible needle with an asymmetric tip interacting with moving soft tissues. These models were designed to provide a quasi-static representation of the whole body of the needle that can be used in a real-time needle steering control scheme. They both use a 1D beam representation for the needle and a local representation for the tissues to keep the computational cost low enough. The first model is inspired from the virtual springs approach presented in section 2.3. This approach is extended to 3D and is used with the addition of a reaction force at the needle tip to take into account an asymmetric geometry of the tip. The second model is a two-body model where the needle interacts with a second 1D beam representing the cut path generated by the needle tip in the tissues.

Note that we use 3D models to account for all the phenomena occurring in practice. It would be possible to maintain the trajectory of the needle base in a 2D plane using a robotic manipulator, however, the motions of the tissues occur in all directions and can not be controlled. Therefore the body of a flexible needle can also move in any direction, such that 3D modeling is necessary.

Needle tissue interaction model with springs

We describe here the first model that we propose and which is inspired from the 2D virtual springs model used in [START_REF] Glozman | Image-guided robotic flexible needle steering[END_REF].

Interaction along the needle shaft: The interaction between the needle and the tissues is modeled locally using n 3D virtual springs placed all along the needle shaft. We define each 3D spring, with index i ∈ [[1, n]], using 3 parameters: a scalar stiffness K i , a rest position p 0,i ∈ R 3 defined in the world frame {F w } (see Fig. 2.5) and a plane P i that contains p 0,i (see Fig. 2.5). The rest position p 0,i of the spring with index i corresponds to the initial location of one point of the tissues when no needle is pushing on it. The plane P i is used to define the point of the needle p N,i ∈ R 3 on which the spring with index i is acting. Each time the model needs to be recomputed, the plane P i is reoriented such that it is normal to the needle and still passes through the rest position p 0,i . This way the springs are only used to model the normal forces F s,i ∈ R 3 applied on the needle shaft, without tangential component. We use an elastic behavior to model the interaction between the needle and the tissues, such that the force exerted by the spring on the point p N,i can be expressed according to

F s,i = -K i (p N,i -p 0,i ).
(2.3) CHAPTER 2. NEEDLE INSERTION MODELING Figure 2.5: Illustration of the mechanical 3D model of needle-tissue interaction using virtual springs and a reaction force at the tip.

The stiffness K i of each spring is computed such that it approximates a given stiffness per unit length K T such that

K i = K T l i , (2.4)
where l i is the length of the needle that is supported by the spring with index i. This length l i can vary depending on the actual distance between the points p N,i-1 , p N,i and p N,i+1 . For simplicity we consider here that the tissue stiffness per unit length K T is constant all along the needle. However it would also be possible to change the value of K T depending on the depth of the spring in the tissues and therefore consider the case of inhomogeneous tissues or variable tissue geometry. In practice this parameter should be estimated beforehand or by using an online estimation method to adapt to unknown stiffness changes. However online estimation of K T will not be considered in this work.

The needle is then modeled by a succession of n + 1 segments such that the extremities of the segments lay on the planes P i of the virtual springs, except for the needle base that is fixed to a needle holder and the needle tip that is free. Each segment is approximated in 3D using a polynomial curve c j (l) of order r so that

c j (l) = M j [ 1 l . . . l r ] T , (2.5)
where j ∈ [[1, n + 1]] is the segment index and c j (l) ∈ R 3 is the position of a point of the segment at the curvilinear coordinate l ∈ [0, L j ], with L j the total length of the segment. The matrix M j ∈ R 3×(r+1) contains the coefficients of the polynomial curve.

Interaction at the tip: The model defined so far is sufficient to take into account the interaction with the tissues along the needle shaft. However the specific interaction at the tip of the needle still needs to be added. We represent all the normal efforts exerted at the tip of the needle with an equivalent normal force F tip and an equivalent normal torque T tip exerted at the extremity of the needle, just before the beginning of the bevel. In order to model a beveled tip, these force and moment are computed using a model of the bevel with triangular loads distributed on each side of the tip, as proposed by Misra et al. [MRS + 10].

Let us define α as the bevel angle, b as the length of the face of the bevel, a as the length of the bottom edge of the needle tip and β as a cut angle that indicates the local direction in which the needle tip is currently cutting in the tissues as depicted in Fig. 2.6. Note that the point O in Fig. 2.6 corresponds to the last extremity of the 3D curve used to represent the needle. The equivalent normal force F tip and torque T tip exerted at the point O can be expressed as

F tip = K T b 2 2 tan(α -β) cos α - K T a 2 2 tan β y, (2.6) T tip = K T a 3 6 tan β - K T b 3 6 tan(α -β) 1 - 3 2 sin(α) 2 x, (2.7)
where x and y are the axis of the tip frame {F t } as defined in Fig. 2.6.

Tip orientation around the shaft: We assume that the orientation of the base frame {F b } (see Fig. 2.5) is known and that the torsional bending of the needle can be neglected. The first assumption usually holds in the case of robotic needle manipulation, where the needle holder can provide a feedback on its pose. The second assumption however can be debated since it has been shown that stiction along the needle can introduce a lag and an hysteresis between the base and tip rotation [START_REF] Abayazid | 3d flexible needle steering in soft-tissue phantoms using fiber bragg grating sensors[END_REF]. However inserting the needle is usually sufficient to break the stiction and reset this lag [START_REF] Reed | Modeling and control of needles with torsional friction[END_REF]. Hence we assume that the orientation of the tip frame {F t } around the tip axis can directly be computed from the base orientation and the needle shape.

Computation of the needle shape: In order to maintain adequate continuity properties of the needle, second order continuity constraints are added, namely defined as c j (L j ) = c j+1 (0), (2.8) dc j dl l=L j = dc j+1 dl l=0 , (2.9)

d 2 c j dl 2 l=L j = d 2 c j+1 dl 2 l=0 .
(2.10)

The total normal force F j at the extremity of the segment j can be calculated from the sum of the forces exerted by the springs located from this extremity to the needle tip, so that

F j = Π j   F tip + n k=j F s,k   , (2.11)
where Π j stands for the projection onto the plane P j . The projection is used to remove the tangential part of the force and to keep only the normal component. This normal force introduces a constant shear force all along the segment and using Bernoulli beam equation we have

EI d 3 c j dl 3 (l) = -F j ,
(2.12)

GENERIC MODEL OF FLEXIBLE NEEDLE

with E the needle Young's modulus and I its second moment of area. Note that in the case of a radially symmetric needle section the second moment of area is defined as

I = Ω x 2 dx dy, (2.13)
where the integral is performed over the entire section Ω of the needle. For a hollow circular needle, I can be calculated from the outer and inner diameters, d out and d in respectively, according to

I = π 64 (d 4 out -d 4 in ).
(2.14)

Finally the moment due to the bevel force gives the following boundary condition:

EI d 2 c n+1 dl 2 l=L n+1 = T tip × z, (2.15)
where T tip is the torque exerted at the tip defined by (2.7) and z is the axis of the needle tip frame {F t } as defined in Fig. 2.6.

In practice we expect real-time performances for the model, so the complexity should be as low as possible. We use here third order polynomials (r = 3) to represent the needle such that each polynomial curve is represented by 12 coefficients. It is the lowest sufficient order for which the mechanical equations can directly be solved. From a given needle base pose and a given set of virtual springs, the shape of the needle can then be computed. The needle model is defined by 12 × (n + 1) parameters, corresponding to the polynomial segments coefficients. The continuity conditions provide 9 × n equations. The fact that the segments extremities have to stay in the planes defined by the springs adds n equations and the springs forces in the planes define 2 × n equations. The base position and orientation give 6 additional boundary equations. The tip conditions also give 6 equations due to the tip force and tip moment. So the final shape of the needle is solved as a linear problem of 12 × (n + 1) unknowns and 12 × (n + 1) equations. In practice we used the Eigen C++ library for sparse linear problem inversion.

Insertion of the needle: During the insertion, springs are added regularly at the tip to account for the new amount of tissues supporting the end of the needle. Once the last segment of the needle reaches a threshold length L thres , a new spring is added at the tip. The rest position of the spring is taken as the initial position of the tissue before the tip has cut through it, corresponding to point A in Fig. 2.6.

The next section presents a second model that we propose, where the successive springs are replaced by a continuous line. A different method is used to solve the needle parameters, allowing a decoupling between the number of elements used to represent the needle and the tissues.

CHAPTER 2. NEEDLE INSERTION MODELING

Needle tissue interaction model using two bodies

In this section we model the interaction between the needle and the tissues as an elastic interaction between two one-dimensional bodies.

Needle and tissues modeling: One of the bodies represents the needle shaft and the other one represents the rest position of the path that was cut in the tissues by the needle tip during the insertion (see Fig. 2.7). Note that the needle body (depicted in red in Fig. 2.7) actually represents the current shape of the path cut in the tissues, while the tissue body (depicted in green in Fig. 2.7) represents this same cut path without taking into account the interaction with the needle, i.e. the resulting shape of the cut after the needle is removed from the tissues. Both bodies are modeled using polynomial spline curves c, such that

c(l) = n i=1 c i (l) , l ∈ [0, L] , (2.16
)

c i (l) = χ i (l) M i      1 l . . . l r      , ( 2.17)
where c(l) ∈ R 3 is the position of a point at the curvilinear coordinate l, L is the total length of the curve, M i ∈ R 3×(r+1) is a matrix containing the coefficients of the polynomial curve c i and χ i is the characteristic function of the curve, that takes the value 1 on the definition domain of the curve and 0 elsewhere. Parameters n and r represent respectively the number of curves of the spline and the polynomial order of the curves. Both can be tuned to find a trade-off between model accuracy and computation time.

In the following we add the subscripts or superscripts N and T on the different parameters to indicate that they respectively corresponds to the needle and tissues.

Computation of the needle shape: For simplicity we assume that the tissues have a quasi-static elastic behavior, i.e. the force exerted on each point of the needle is independent of time and proportional to the distance between this point and the rest cut path. This should be a good approximation as long as the needle remains near the rest cut path, what should be ensured in practice to avoid tissue damage. We note K T the interaction stiffness per unit length corresponding to this interaction. Given a segment of the needle between curvilinear coordinates l 1 and l 2 , the force exerted on it by the tissues can thus be expressed as

F (l 1 , l 2 ) = -K T l 2 l 1 c N (l) -c T (l)dl.
(2.18)

GENERIC MODEL OF FLEXIBLE NEEDLE

Figure 2.7: Illustration of the whole needle insertion model (left) and zoom on the tip for different tip geometries (right). Needle segments are in red and the rest position of the path cut in the tissues is in green. New segments are added to the cut path according to the location of the cutting edge of the tip. + 09] that the quasi-totality of the energy stored in the needle-tissue system consists in the bending energy of the needle E N and the deformation energy of the tissues E T . We use the Rayleigh-Ritz method to compute the shape of the needle which minimizes the sum of these two terms.

It has been shown in previous work [MRS

According to the Euler-Bernouilli beam model, the bending energy E N of the needle can be expressed as

E N = EI 2 L N 0 d 2 c N (l) dl 2 2 dl, (2.19)
where E is the Young's modulus of the needle, I is its second moment of area (see the definition in (2.13)) and L N is its length. By tuning the parameters E and I according to the real needle, both rigid and flexible needles can be represented by this model.

CHAPTER 2. NEEDLE INSERTION MODELING

The energy stored in the tissues due to the needle displacement can be expressed as

E T = K T 2 L T 0 c N (L f ree + l) -c T (l) 2 dl, (2.20)
where L f ree is the length of the free part of the needle, i.e. from the needle base to the tissue surface, and L T is the length of the path cut in the tissues.

We add the constraints imposed by the needle holder, which fix the needle base position p b and direction d b , so that

c N (0) = p b , (2.21) dc N dl (0) = d b . (2.22)
Continuity constraints up to order two are also added on the spline coefficients

c N i (l i ) = c N i+1 (l i ), (2.23) dc N i dl l=l i = dc N i+1 dl l=l i , (2.24)
d 2 c N i dl 2 l=l i = d 2 c N i+1 dl 2 l=l i , (2.25)
where l i is the curvilinear coordinate along the needle spline corresponding to the end of segment c N i and the beginning of segment c N i+1 . In order to take into account the length of the tip, which can be long for example for pre-bent tips or beveled tips with small bevel angle, the tip is modeled as an additional polynomial segment added to the needle spline, as can be seen in Fig. 2.7. The corresponding terms are added to the bending energy (2.19) and tissue energy (2.20), similarly to the other segments.

The system is then solved as a minimization problem under constraints, expressed as

min m E N + E T Am = b, (2.26)
where m is a vector stacking all the coefficients of the matrices M i and with matrix A and vector b representing the constraints (2.21) to (2.25).

In practice this minimization problem reduces to the inversion of a linear system, so that we also used the Eigen C++ library for sparse linear problem inversion.

Tip orientation around the shaft: Similarly to the previous model (see section 2.4.1), we assume that there is no lag between the tip rotation and the base rotation along the needle shaft. This way the orientation of the tip can be computed from the orientation of the base and the shape of the needle. A more complex modeling of the torsional compliance of the needle could however be necessary in the case of a pre-bent tip needle for which the shape of the tip could cause a higher torsional resistance.

Insertion of the needle: As the needle progresses in the tissues and the length of the cut path increases, we update the modeled rest cut path by adding new segments to the spline curve. Each time the model is updated, if the needle was inserted more than a defined threshold L thres , a new segment is added such that its extremity corresponds to the location of the very tip of the needle, i.e. where the cut occurs in the tissues. This way the model can take into account the specific geometry of the needle tip. In the case of a symmetric tip, the cut path will stay aligned with the needle axis. On the other hand it will be shifted with respect to the center line of the needle shaft when considering an asymmetric tip, as is depicted in Fig. 2.7, leading to the creation of a force that will pull the needle toward the direction of the cut.

It can be noted that external tissue deformations can be taken into account with this kind of modeling. Indeed, deformations of the tissues created by external sources, like tissue manipulation or natural physiological motions (heartbeat, breathing, . . . ), induce modifications of the shape and position of the rest cut path. This, in turn, changes the shape of the needle via the interaction model. External tissue motions will be further studied in the section 3.5 of the next chapter.

Another advantage of this model is that the number of polynomial curves of the needle spline is fixed and is independent of the number of curves of the tissue spline. This leads to a better control over the computational complexity of the model compared to the virtual springs approach presented in section 2.4.1. Using the virtual springs model, the number of parameters to compute increases as the needle is inserted deeper into the tissues, due to the progressive addition of springs and needle segments. This is an important point for the use of the model in a real-time control framework.

In the next section we compare the performances of both models in terms of accuracy of the obtained tip trajectories and computation time.

Validation of the proposed models

In this section we compare the performances of the models defined previously in terms of accuracy of the representation of the needle behavior. We compare the simulated trajectories of the needle tip obtained with both models to the real trajectories of a needle inserted in soft tissues under various motions of the needle base. We first describe the experiments performed to acquire the trajectories of the base and tip of the needle and then we provide the comparison of these trajectories to the ones generated using both models.

Experimental conditions (setup in the Netherlands): We use the needle insertion device (NID) attached to the end effector of the UR3 robot to insert the Aurora biopsy needle in a gelatin phantom, as depicted in Fig. 2.8. The needle is 8 cm outside of the NID and its length does not vary during the insertion. The position of the needle tip is tracked and recorded using the Aurora electromagnetic (EM) tracker embedded in the tip and the field generator. The pose of the needle base, at the tip of the NID (center of frame {F b } in Fig. 2.8), is recorded using the odometry of the UR3 robot. The phantom has a Young modulus of 35 kPa and is maintained fixed during the experiments.

Experimental scenarios: Different trajectories of the needle base are performed to test the models in any possible direction of motion. The different insertions are performed at the center of the phantom, such that they do not cross each other. We use 12 different insertion scenarios and repeat each scenario 3 times, leading to a total of 36 insertions. Each scenario is decomposed as follows. The needle is first placed perpendicular to the surface of the phantom such that the tip barely touches the surface. Then the needle is inserted 1 cm in the phantom, by translating the robot along the needle axis. Then a motion of the needle base is applied before restarting the insertion for 5 cm. The applied motion is expressed in the frame of the needle base An example of the measured tip trajectories for each type of base motions can be seen in solid lines in Fig. 2.9 to 2.13. The tip position is expressed in the initial frame of the tip, at the surface of the phantom.

Generation of model trajectories:

In order to generate the different trajectories of the needle tip using both models, we first set their parameters according to the physical properties of the needle. The needle length is set to 8 cm and the other parameters are set according to the properties of the Aurora needle given in Table 1.1. The polynomial order of the curves is set to r = 3 for both models and the length threshold defining the addition of a virtual spring or tissue spline segment is set to L thres = 1 mm. The length of the needle segments for the two-body model is set to 1 cm, resulting in a total of n = 8 segments. We recall that the number of segments for the virtual springs model varies with the number of springs added during the insertion.

One tip trajectory is then generated for both models and each experiment by applying the motion of the base that is recorded during the experiment to the base of the model. The value of the model stiffness per unit length K T of both models is optimized separately such that the final error between the simulated tip positions and the measured tip positions is minimized. Since the insertions are performed in the same phantom and at similar locations, the same value of K T is used for all experiments. The best fit is obtained with K T = 49108 N.m -2 for the two-body model and K T = 56868 N.m -2 for the virtual springs model. As mentioned in previous section, in clinical practice this parameter can be difficult to estimate beforehand and would certainly need to be estimated online.

It can be observed in Fig. 2.9 to 2.13 that the tip trajectories measured for similar base motions in symmetric directions are not symmetric. This is due to a misalignment between the axis of the NID, in which are performed the motions, and the real axis of the needle. This misalignment corresponds to a rotation of 1.0 • around axis 0.5 0.86 0 T in the base frame {F b }.

Similarly an orientation error of 4.1 • is observed between the orientation of the NID around the needle axis and the orientation of the bevel. A correction is thus applied to the needle base pose measured from the robot odometry to obtain the pose that is applied to the modeled needle base.

An example of the simulated tip trajectories for each type of base motions can be seen in Fig. 2.9 to 2.13, with long-dashed lines for the virtual springs model and short-dashed lines for the two-body model.

z tip (cm) 0 • +3 • -3 • (b)
z tip (cm) 0 • +3 • -3 • (b)
Figure 2.12: Tip position obtained when a rotation is applied around the y axis of the base frame between two insertion steps along the z axis. Measures are shown with solid lines, virtual springs model with long-dashed lines and two-body model with short-dashed lines.

Results: We can observe that both models follow the global behavior of the needle during the insertion. Let us first focus on the effect of the asymmetry of the beveled tip.

We can see that during an insertion without lateral base motions, the deviation due to the bevel is well taken into account, as for example in

z tip (cm) 0 • +90 • -90 • 180 • (b)
Figure 2.13: Tip position obtained when a rotation is applied around the z axis of the base frame between two insertion steps along the z axis. Measures are shown with solid lines, virtual springs model with long-dashed lines and two-body model with short-dashed lines. Fig. 2.9b. We observe the same kind of constant curvature trajectories that are usually obtained with kinematic models. However it can also be seen that during the first few millimeters of the insertion, a lateral translation of the tip occurs, which does not fit the constant curvature trajectory appearing later. This effect is due to the fact that the bevel is cutting laterally while the needle body is not yet embedded in the gelatin. The reaction force generated at the bevel is thus mostly compensated by the stiffness of the needle, which is low due to the length of the needle. This effect can usually be reduced in practice by using a sheath around the body of the flexible needle, such that it can not bend outside the tissues [START_REF] Webster | Design considerations for robotic needle steering[END_REF]. However in a general case, this kind of effect is not taken into account by kinematic models, while it can be represented using our mechanics-based models.

Let us now consider the influence of lateral base motions on the behavior of the needle. We can see that the tip trajectory is modified in the same manner for both models and follows the general trajectory of the real needle tip. Therefore both models can be used to provide a good representation of the whole 3D behavior of a flexible needle inserted in soft tissues. This is a great advantage over kinematic models, which do not consider lateral base motions at all.

Concerning the accuracy of the modeling, some limitations seem to appear when the base motion tends to push the surface of the bevel against the tissues. This is for example the case for a positive translation along the y axis of the base frame (green curves in Fig. 2.10b) or a negative rotation around the x axis (blue curves in Fig. 2.11b). In these cases both models seem to amplify the effect of the base motions on the following trajectory of the tip. However this could also be due to the experimental conditions. A small play between the needle and the NID could indeed cause an attenuation of the motion transmitted to the needle base.

1 2 3 4 5 6 7 N o n e T x = + 2 m m T x = - 2 m m T y = + 2 m m T y = - 2 m m R x = + 3 • R x = - 3 • R y = + 3 • R y = - 3 • R z = + 9 0 • R z = -9 0 • R z = + 1 8 0 • M e a n

Accuracy comparison:

Let us now compare to each other the performances of both models in terms of accuracy. The absolute final error between the tip positions simulated by the models and the tip positions measured during the experiments are summarized in Fig. 2.14. Mean values are provided across the 3 insertions performed for each scenario. The average position error over the insertion process is provided as well in Fig. 2.15. The average is taken over the whole insertion and across the 3 insertions performed for each scenario.

We can see that the two-body model provides in each scenario a better modeling accuracy on the trajectory of the needle tip. While both models tends to give similar results in the general case, the virtual springs model seems to particularly deviate from the measures when rotations around the needle axis are involved. This is also clearly visible in Fig. 2.13a. Several reasons may be invoked to explain this result. First it is possible that the discrete nature of the springs has a negative effect on the modeling accuracy compared to a continuous modeling of the load applied on the needle shaft. However we believe that this effect is not predominant here since the thresholds chosen for both models (distance between successive springs in one case and length of the segment added to the cut path spline in the other case) were the same and had small values compared to the curvature of the needle. The second possible reason is that the model to compute the force and torque at the needle tip is not the best way to represent the interaction with the tissues. Indeed, the computation of the continuous loads applied on the sides of the tip does not take into account the real 3D shape of the tip, which has a circular section. The force magnitude is also independent of the orientation of the bevel, which might not be true during the rotation of the needle around its axis, leading to a wrong orientation of the tip when the insertion restarts. Concerning the use of the model in clinical practice, we can see that the final tip positioning error obtained with the two-body model is around 2 mm in each case. This can be sufficient to reach tumors with standard size in open-loop control. However this assumes that the stiffness parameter K T was well estimated beforehand, which might be difficult in practice and could require the use of an online estimation method.

0 0.5 1 1.5 2 2.5 3 N o n e T x = + 2 m m T x = - 2 m m T y = + 2 m m T y = - 2 m m R x = + 3 • R x = - 3 • R y = + 3 • R y = -3 • R z = + 9 0 • R z = -9 0 • R z = + 1 8 0 • M e a n
Computation time comparison: Let us finally compare the time required to compute both models depending on the number of elements used to represent the tissues, i.e. the number of springs for the virtual springs model and the number of tissue spline segments for the two-body model. The computation times are acquired during a simulation of the needle insertion using the same parameters as in the previous experiments. The results are depicted in Fig. 2. 16.

It is clearly visible that the number of virtual springs increases the computation time, as could be expected from the fact that the number of needle parameters to compute directly depends on the number of springs. On the contrary the number of tissue spline segments of the two-body model does not have a significant influence on the computation time since the number of parameters of the needle spline is fixed and chosen independently, hence the size of the linear problem to solve is also fixed. This is a clear advantage of the two-body model since the computation time can then be independent of the state of the insertion and can also be tuned beforehand to obtain the desired real-time performances. Note that we obtained a computation time around 2 ms, which is sufficient for real-time computation in our case, however this could greatly vary and be tuned depending on the available hardware and the number of segments chosen to represent the needle.

In conclusion, we will use the two-body model in the following of the experiments, because it can provide an accurate estimation of the 3D behavior of the needle and can be used for real-time processing due to its deterministic computation time. It is also easier to adapt to different kinds of tip geometry and the motion of the tissue spline can be used to model external displacement of the tissues.

Conclusion

We presented a review of needle-tissue interaction models separated into three categories corresponding to different cases of use. Typically, kinematic models ignore the interaction of the needle body with the surrounding tissues and only consider the trajectory of the needle tip. Hence they are computationally inexpensive and are well adapted for real-time control of the insertion of needles with asymmetric tips. On the other side, complete models of the needle and tissues based on finite element modeling offer an accurate but complex modeling of insertion procedures. They usually require far more resources, which limits their use to applications where real-time performances are not a priority, such as needle insertion pre-planning or surgical training simulations.

In-between these two categories are mechanics-based models, which uses local approaches to model the full behavior of a needle being inserted in soft tissues while keeping aside the full modeling of the tissues. They provide a more complete modeling than kinematic models while maintaining good performances for a real-time use. In section 2.4 we have proposed two 3D mechanics-based models that give a good representation of the 3D behavior of a needle during its insertion is soft tissues. In particular, the two-body model that we designed offers a good accuracy for all kinds of motions applied to the base of the needle. Its complexity can also be chosen and is constant during the insertion, which allows tuning the required computation time to achieve desired real-time performances. Therefore we will use this model as a basis for a real-time needle steering framework in chapter 4. The accuracy of the modeling of the needle tip trajectory is however dependent on the estimation of the interaction stiffness between the needle and the tissues. Real tissues are usually inhomogeneous and pre-operative estimation of the stiffness can be difficult, such that online estimation techniques would have to be studied for a use in real clinical practice.

In this chapter we only considered the case of stationary tissues, while our model can also handle moving tissues by modifying the position of the curve representing the tissues (rest cut path). Contrary to the motions of the needle base, which can be controlled by the robotic manipulator, the motions of the patient can not be controlled. Therefore an information feedback is necessary to estimate these motions and update the state of the model 2.6. CONCLUSION accordingly. Visual feedback is usually used in current practice to monitor the whole needle insertion procedure and provide a way to see both the needle and a targeted region. Hence we will focus on this kind of feedback in order to design an update method for our model. In particular, 3D ultrasound (US) can provide a real-time feedback on the whole shape of the needle, which can be used to ensure that our model stays consistent with the real state of the insertion procedure. A first step is to design an algorithm to extract the localization of the needle body in the US volumes, which is a great challenge in itself, due to the low quality of the data provided by the US modality. This will be a first point of focus of the next chapter 3. The performances of needle tracking algorithms in terms of accuracy and computation time can be greatly improved by using a prediction of the needle location. Therefore we will also use our model for this purpose, since we have shown that it could predict the trajectory of the needle with a good accuracy.

Chapter 3

Needle localization using ultrasound

In this chapter we focus on the robust detection and tracking of a flexible needle in 3D ultrasound (US) volumes. In order to perform an accurate realtime control of a flexible needle steering robotic system, a feedback on the localization of the needle and the target is necessary. The 3D US modality is well adapted for this purpose thanks to its fast acquisition compared to other medical imaging modalities and the fact that it can provide a visualization of the entire body of the needle. However the robust tracking of a needle in US volume is a challenging task due to the low quality of the image and the artifacts that appear around the needle. Additionally, even though intraoperative volumes can be acquired, the needle can still move between two volume acquisitions due to its manipulation by the robotic system or the motions of the tissues. A prediction of the needle motion can thus be of a great help to improve the performances of needle tracking in successive volumes.

We first provide in section 3.1 several points of comparison between the imaging modalities that are used in clinical practice to perform needle insertions and we motivate our choice of the 3D US modality. We then describe the principles of US imaging and the techniques used to reconstruct the final 2D images or 3D volumes in section 3.2. We present in section 3.3 a review of the current methods used to detect and track a needle in 2D or 3D US. We then propose a new needle tracking algorithm in 3D US volumes that takes into account the natural artifacts observed around the needle. We focus on the estimation of the motions of the tissues in section 3.5 and propose a method to update the needle model that we designed in the previous chapter using different measures available on the needle. Tests and validation of the method are then provided in section 3.6. The updated model is then used to improve the performances of the needle tracking across a sequence of US volumes.

The work presented in this chapter on the model update from visual feedback was published in an article presented in international conference [START_REF] Chevrie | Online prediction of needle shape deformation in moving soft tissues from visual feedback[END_REF].

Introduction

Needle insertion procedures are usually performed under imaging feedback to ensure the accuracy of the targeting. Many modalities are available, among which the most used ones are magnetic resonance imaging (MRI), computerized tomography (CT) and ultrasound (US). In the following we present a comparison of these modalities on several aspects that has led us to consider the use of the US modality instead of the others.

Image quality:

The main advantage of MRI and CT is that they provide high contrast images of soft tissues in which the targeted lesion can clearly be distinguished from the surrounding tissues. On the other side, the quality of US images is rather poor due to the high level of noise and interference phenomena. However MRI and CT are sensitive to most metallic components, that creates distortions in the images. The presence of a metallic needle in their field of view is thus a source of artifacts that are disturbing for MRI [SCI + 12] or CT [SGS + 16] when compared to US artifacts [START_REF] Reusz | Needlerelated ultrasound artifacts and their importance in anaesthetic practice[END_REF]. This alleviates the main drawback of the US modality.

Robotic design constraints: MRI and CT have additional practical limitations compared to US imaging, since their scanners are bulky and require a dedicated room. These scanners are composed of a ring inside which the patient is placed for the acquisition, which reduces the workspace and accessibility to the patient for the surgical intervention. This adds heavy constraints on the design of robotic systems that can be used in these scanners [MGB + 04] [ZBF + 08], in addition to the previously mentioned incompatibility with metallic components, that can even cause security issues in the case of MRI.

On the other side, US probes are small, can easily be moved by hand and the associated US stations are easily transportable to adapt to the workspace. Additionally they do not pose particular compatibility issues and can thus be used with a great variety of robotic systems.

Acquisition time:

The acquisition time of MRI and CT is typically long compared to US and makes them unsuitable for real-time purposes. Using non real-time imaging requires to perform the insertion in many successive steps, alternating between image acquisitions and small insertions of the needle, as classically done with MRI [MvdSvdH + 14] or CT [SHvK + 17]. In addition to the increased duration of the intervention, patients are often asked to hold their breath during the image acquisition to avoid motion blur in the image. Therefore, discrepancies arise between the real position of the needle and the target and their position in the image because of the motions of the tissues as soon as the patient restarts breathing. For these reasons, real-time imaging is preferred and can be achieved using the US modality. High acquisition rates are usually obtained with 2D US probes, as tens of images can typically be acquired per second. A 3D image, corresponding to an entire volume of data, can also be acquired at a fast rate using matrix array transducers or at a lower frame rate using more conventional motorized 3D US probes.

In conclusion, US remains the modality of choice for real-time needle insertion procedures [START_REF] Chapman | Visualisation of needle position using ultrasonography[END_REF]. Hence in the following of this chapter we will focus on the detection and tracking of a needle using the US feedback acquired by 3D probes. In the next section we present the general principles of US imaging.

Ultrasound imaging

Physics of ultrasound

Ultrasound (US) is a periodic mechanical wave with frequency higher than 20 kHz that propagates by producing local changes of the pressure and position in a medium. The principle of US imaging is to study the echos reflected back by the medium after that an initial US pulse has been sent.

Wave propagation: Most imaging devices assume that soft tissues behave like water, due to the high proportion of water they contain. The speed c of US waves in liquids can be calculated according to the Newton-Laplace equation

c = K ρ , (3.1)
where K is the bulk modulus of the medium and ρ its density. Although variations of the local density of the tissues introduce variations of the speed of ultrasound, it is most of the time approximated by a constant c =1540 m.s -1 . When the wave encounters an interface between two mediums with different densities, a part of the wave is reflected back while the rest continues propagating through the second medium. The amplitudes of the reflected and transmitted waves depend on the difference of densities. This is the main phenomenon used in US imaging to visualize the variations of the tissue density. Image formation: An US transducer consists in an array of small piezoelectric elements. Each element can vibrate according to an electric signal sent to them, creating an US wave that propagates through the medium in the form of a localized beam. Each beam defines a scan line on which the variation of the tissue density can be observed. The elements also act as receptors, creating an electric signal corresponding to the mechanical deformations applied to them by the returning echos. These electric signals are recorded for a certain period of time after a short sinusoidal pulse was applied to an element, giving the so-called radio frequency signal.

Considering an interface that is at a distance d from the wave emitter, an echo will be observed after a time

T = 2d c , (3.2)
corresponding to the time needed by the pulse to propagate to the interface and then come back to the transducer. The position corresponding to an interface can thus directly be calculated from the delay between the moment the US pulse was sent and the moment the echo was received by the transducer, as illustrated in Fig. 3.1. The radio frequency signal can then be transformed into a suitable form for an easy visualization of the tissue density variations along each scan line.

Acquisition frequency:

The acquisition frequency depends on the number of piezoelectric elements n p and the desired depth of observation d o , i.e. the length of the scan lines. The radio frequency signals corresponding to 3.2. ULTRASOUND IMAGING each scan line must be recorded one after another in order to avoid mixing the echos corresponding to adjacent scan lines. The time T line required to acquire the signal along one scan line is given by

T line = 2d o c . (3.3)
The total time T acq of the 2D image acquisition is then

T acq = n p T line . (3.4)
A typical acquisition using a transducer with 128 elements and an acquisition depth of 10 cm would take 16.6 ms, corresponding to a frame rate of 60 images per second. This is a fast acquisition rate that can be considered real-time for most medical applications.

Image resolution:

The axial resolution of the US imaging system is the minimum distance between two objects in the wave propagation direction that allows viewing them as two separate objects in the image. This directly corresponds to the wavelength of the wave propagating in the medium. Wavelength λ and frequency f are directly related to each other by the speed of the wave according to λ = c f . (3.5) Therefore a higher axial resolution can be achieved through a higher frequency. Standard US systems usually use a frequency between 1 MHz and 20 MHz, corresponding to an axial resolution between 1.54 mm and 77 µm.

The lateral resolution is the minimum distance between two objects perpendicular to the wave propagation direction that allows viewing them as two separate objects in the image. A threshold value for this resolution is first set by the distance between the different scan lines, which depends on the geometry of the transducer. For linear transducers, the piezoelectric elements are placed along a line, such that all scan lines are parallel and the threshold directly corresponds to the distance between the elements. For convex transducers, the elements are placed along a circular surface, such that the scan lines are in a fan-shape configuration and diverge from the transducer, as illustrated in Fig. 3.2. The threshold thus corresponds to the distance between the elements at the surface of the transducer but then grows with the depth due to the increasing distance between the scan lines. Hence, this configuration allows a larger imaging region far from the transducer, but at the expense of a resolution decreasing with the depth.

However another factor that determines the real lateral resolution is the width of the US beam. This varies with depth and depends on the size of the piezoelectric elements and the wave frequency. The wave focuses into a narrow beam only for a short distance from the emitter, called the near zone or Fresnel's zone. Then the wave tends to diverge, leading to a wide beam in the far zone or Fraunhofer's zone, as illustrated in Fig. 3.3.

The width of the beam in the near zone is proportional to the size of the piezoelectric element, while the length of the zone decreases with this size. The length of the near zone can be increased by using a higher frequency. The lateral resolution is also often modified by using several adjacent elements with small delays instead of only one at a time. This generates a beam focused at a specified depth, but also causes the far zone width to increase faster after the focus depth.

Similarly, an out of plane resolution can be defined, which determines the actual thickness of the tissue slice that is visible in the image. This resolution also varies with depth depending on the size of the piezoelectric elements and the frequency, as illustrated in Fig. 3.3. An acoustic lens is often added to the surface of the probe to focus the beams at a given depth.

ULTRASOUND IMAGING

Observation limitations: Other factors can modify the quality of the received US wave.

Attenuation: The viscosity of soft tissues is responsible for a loss of energy during the US wave propagation [START_REF] Wells | Absorption and dispersion of ultrasound in biological tissue[END_REF]. This loss greatly increases with the wave frequency, such that a trade-off has to be made between the spatial resolution and the attenuation of the signal.

Speckle noise: Due to the wave characteristics of US, diffraction and scattering also occur when the wave encounters density variations in the tissues that have a small size compared to its wavelength. The US beam is then reflected in many directions instead of one defined direction. This results in textured intensity variations in the US images known as speckle noise, as can be seen in Fig. 3.1. While it has sometimes been used for tissue tracking applications as in [START_REF] Krupa | Real-Time Tissue Tracking with B-Mode Ultrasound Using Speckle and Visual Servoing[END_REF][KFH09], speckle noise is generally detrimental to a good differentiation between the different structures in the tissues, such that filtering is often performed to reduce its intensity.

Shadowing: At the level of an interface between two media with very different densities it can be observed that the US wave is almost entirely reflected back to the transducer. The intensity of the transmitted signal is then very low, such that the intensity of the echos produced by the structures that are behind the interface are greatly reduced. This causes the appearance of a shadow in the US image that greatly limits the visibility of the structures behind a strong reflector. This can be seen on the bottom of the US image in Fig. 3.1, which is mostly dark due to the presence of reflective interfaces higher in the image.

Particular artifacts can also appear around a needle due to its interaction with the US wave. These artifacts can greatly affect the appearance of the needle in 2D or 3D US images, such that they should be taken into account to accurately localize the needle. This kind of artifacts will be the focus of section 3.3.1. Now that we have seen the principles of US signals acquisition, we describe in the following how the acquired data are exploited to reconstruct the final image or volume.

Reconstruction in Cartesian space

The radio frequency signal acquired by the piezoelectric elements must be converted into a form that is suitable for the visualization of the real shape of the observed structures in Cartesian space. This conversion should take into account the natural attenuation of the ultrasound (US) signal during its travel in the tissues as well as the geometric arrangement of the different scan lines. We describe in the followings the process that is used to transform the radio frequency signal into a 2D or 3D image.

Reconstruction of 2D images

We first consider the case of a reconstructed 2D image, called B-mode US image.

The signal is first multiplied by a depth-dependent gain to compensate for the attenuation of the US signal during its travel through the tissues. The amplitude of the signal is then extracted using envelop detection. This removes the sinusoidal shape of the signal and only keeps the part that depends on the difference between the density of the tissues. The signal is then sampled to enable further digital processing. The sampling frequency is chosen to respect the Nyquist sampling criterion, i.e. it should be at least twice as much as the frequency of the US wave. This frequency is typically of 20 MHz or 40 MHz for current 2D US probes. A logarithmic compression of the level of intensity is then usually applied to the samples to facilitate the visualization of high and low density variations on the same image. The samples can be stored in a table with each line corresponding to the samples of a same scan line. The resulting image is called the pre-scan image.

The samples need then to be mapped to their corresponding position in space to reconstruct the real shape of the 2D slice of tissue being observed, which constitute the post-scan image. Let N s be the number of samples along a scan line and N l the number of scan lines. Each sample can be attributed two indexes i ∈ [[0, N s -1]] and j ∈ [[0, N l -1]] corresponding to their placement in the pre-scan image, with j the scan line index and i the sample index on the scan line. The shape of the reconstructed image depends on the geometry of the arrangement of the piezoelectric elements. For linear probes, the piezoelectric elements are placed on a straight surface, such that the scan lines are parallel to each other. In this case the coordinates x and y of a sample in the post-scan image (see Fig. 3.4) can directly be calculated with a scaling and offset such that

x = L s i, (3.6) y = L p j - N l -1 2 , (3.7)
where L s is the physical distance between two samples on a scan line and L p is the distance between two piezoelectric elements of the transducer. The physical distance L s between two samples on a scan line depends on the sampling frequency f s such that For a convex probe with radius R, the physical position of a sample in the probe frame can be expressed in polar coordinates (r, θ) according to

L s = c f s . ( 3
r = R + L s i, (3.9) θ = L p R j - N l -1 2 . (3.10)
This can be converted to Cartesian coordinates using

x = r cos(θ), (3.11)
y = r sin(θ).

(3.12)

In practice the post-scan image is defined in Cartesian coordinates with an arbitrary resolution, such that the mapping should actually be done the opposite way. Each pixel (u, v) in the image corresponds to a physical position (x, y) in the imaging plane of the probe such that

u = x -x min s , (3.13) v = y -y min s , (3.14)
where (x min , y min ) is the position of the top left corner of the image in the probe frame, as depicted in Fig. 3.4, and s is the pixel resolution of the image. The value of each pixel is computed by finding the value in the prescan image at position (i, j) corresponding to the physical position (x, y).

For linear probes this is computed according to

i = x L s , (3.15) j = N l -1 2 + y L p , (3.16)
while for convex probes it results in

i = x 2 + y 2 -R L s , (3.17) j = N l -1 2 + atan2(y, x)R L p . (3.18)
In practice this process gives non-integer values for i and j, while the pre-scan data are only acquired for integer values of i and j. Therefore an interpolation process is necessary to compute the actual value I post (u, v) of the pixel in the post-scan image I post from the available values in the pre-scan image I pre . Different interpolation techniques can be used:

• Nearest neighbor interpolation: the pixel value is taken as the value of the closest prescan sample:

I post (u, v) = I pre ([i], [j]), (3.19)
where [.] denotes the nearest integer operator. This process is fast but leads to a pixelized aspect of the post-scan image.

• Bi-linear interpolation : the pixel value is computed using a bi-linear interpolation between the four closest neighbors:

I post (u, v) = (1-a)(1-b) I pre ( i , j ) + a (1-b) I pre ( i + 1, j ) +(1-a) b I pre ( i , j +1) + a b I pre ( i + 1, j +1), (3.20) with a = i -i , (3.21) b = j -j , (3.22)
and where . denotes the floor operator. This process provides a smoother resulting image while still remaining relatively fast to compute.

• Bi-cubic interpolation : the pixel value is computed using a polynomial interpolation between the 16 closest neighbors. This process provides a globally smoother resulting image and keeps a better definition of edges than bi-linear interpolation. However it involves longer computation times.

ULTRASOUND IMAGING

Reconstruction of 3D volumes

The 2D US modality only provides a feedback on the content of a planar zone in the tissues. In order to visualize different structures in 3D, it is necessary to move the US probe along a known trajectory and reconstruct the relative 3D position of the structures. In the case of needle segmentation, it is possible that only a section of the needle is visible, when the needle is perpendicular to the imaging plane. In this case it is difficult to know exactly which point along the needle corresponds to this visible section. Even when a line is visible in the image, it is possible that the needle is only partially visible and partially out of the imaging plane. It can lead to erroneous conclusions about the real position of the needle tip, which could lead to dramatic outcomes if used as input for an automatic needle insertion algorithm. To alleviate this issue, automatic control of the probe position can be performed to maintain the visibility of the needle in the image [CKM13] [START_REF] Mathiassen | Visual servoing of a medical ultrasound probe for needle insertion[END_REF]. However this is not always possible if the needle shaft does not fit into a plane due to its curvature, which is often the case for the flexible needles that we use in the following. Three-dimensional US probes have been developed to provide a visual feedback on an entire 3D region of interest [START_REF] Huang | A review on real-time 3d ultrasound imaging technology[END_REF]. This way entire 3D structures can be visualized without moving the probe. Two main technologies are available: matrix array transducers and motorized transducers.

Matrix array transducers:

This technology is a 3D version of the classical 2D transducers. It uses a 2D array of piezoelectric elements placed on a surface instead of only one line. Similarly to 2D probes, the 3D matrix probes can be linear or bi-convex depending on whether the surface is planar or curved. They also provide the same fast acquisition properties, with a volume acquisition rate that is proportional to the number of elements of the array. However, due to the complexity of manufacturing, current probes only have a limited number of piezoelectric elements in each direction compared to 2D transducers, which limits the resolution and field of view that can be achieved.

Motorized transducers: Also known as wobbling probes, these probes consist in a classical 2D US transducer attached to a mechanical part that applies a rotational motion to it. A series of 2D US images are acquired and positioned into a volume using the known pose of the transducer at the time of acquisition. In the following we consider the case of a sweeping motion such that the imaging plane of the transducer moves in the out of plane direction. In this case, the resolution of the volume is different in all directions. It corresponds to the resolution of the transducer in the imaging plane, while the resolution in the sweeping direction depends on the frame rate of the 2D transducer and the velocity of the sweeping. The acquisition rate is also limited by the duration of the sweeping motion, such that a trade-off has to be made between the resolution in the sweeping direction and the acquisition rate. Since an indirect volume scanning is made, some motion artifacts can appear in the volume, due to the motion of the tissues or the probe during the acquisition.

Similarly to the 2D case, a post-scan volume with arbitrary voxel resolution can be reconstructed from the acquired pre-scan data. Each voxel (u, v, w) in the post-scan volume corresponds to a physical position (x, y, z) in the probe frame, such that

u = x -x min s , (3.23) v = y -y min s , (3.24) w = z -z min s , (3.25)
where (x min , y min , z min ) is the position of the top left front corner of the reconstructed volume in the probe frame and s is the voxel resolution of the image. The value of each voxel is computed by finding the value in the prescan image at position (i, j, k) corresponding to the physical position (x, y, z).

It should be noticed that the center of the transducer does usually not lie on the axis of rotation of the motor, such that all acquired scan lines do not cross at a common point, which increases the complexity of the geometric reconstruction of the volume.

We define R m the radius of the circle described by the center point on the transducer surface during the sweeping. The position (x, y, z) of a point in space will be defined with respect to the center of rotation O m of the motor, which is fixed in the probe frame, while the center of the transducer O t can translate, as depicted in Fig. 3.5. This leads to

x = (r cos(θ) -R + R m ) cos(φ), (3.26) y = r sin(θ), (3.27) z = (r cos(θ) -R + R m ) sin(φ), (3.28)
where r and θ are the polar coordinates of the sample in the transducer frame and φ is the current sweeping angle of the motor (see Fig 3 .5). The volume can be reconstructed by assuming that equiangular planar frames are acquired. However, since the scan lines are acquired one after another by the transducer, the sweeping motion introduces a small change in the direction of successive scan lines. Therefore the scan lines are not co-planar and some motion artifacts can appear in the reconstructed volume. In order to avoid these artifacts, we reconstruct the volume using the exact orientation of the scan lines, such that r, θ and φ can be computed according to [START_REF] Lee | Intensity-based visual servoing for non-rigid motion compensation of soft tissue structures due to physiological motion using 4d ultrasound[END_REF]:

r = R + L s i, (3.29) θ = L p R j - N l -1 2 , (3.30) φ = δφ k + j N l - N f N l -1 2N l , (3.31)
where N f is the number of frames acquired during one sweeping motion, δφ is the angular displacement of the transducer between the beginning of two frame acquisitions and is equal to 1 if the sweeping motion is performed in the positive z direction and -1 in the negative one.

Finally the position (i, j, k) in the pre-scan data corresponding to the CHAPTER 3. NEEDLE LOCALIZATION USING ULTRASOUND physical position (x, y, z) in the probe frame can be calculated according to

i = r -R L s , (3.32) j = N l -1 2 + R L p θ, (3.33) k = N f N l -1 2N l - j N l + φ δφ , (3.34) with r = R -R m + x 2 + z 2 2 + y 2 , (3.35) θ = atan2 y, R -R m + x 2 + z 2 , (3.36) φ = atan2(z, x). (3.37)
As in the 2D case, voxel interpolation is necessary. The same techniques can be used: nearest neighbor interpolation still requires only one voxel while tri-linear interpolation requires 8 voxels and tri-cubic interpolation requires 64 voxels. Due to the high number of voxels and the increased dimension of the interpolation, the conversion to post-scan can be time consuming and often requires hardware optimization to parallelize the computations and achieve reasonable timings.

Once the US volume has been reconstructed, the different structures present in the tissues can then be observed. In particular, the real 3D shape of a needle can be detected in Cartesian space.

Needle detection in ultrasound

Robust needle tracking using ultrasound (US) has the potential to make possible robotic needle guidance. Due to the high density of a metallic needle, a strong echo is generated, such that the needle appears as a very bright line in US images. However detecting a needle in US images is still a challenging task due to the overall noisy nature of the images.

In this section we present the common factors that may hinder a detection algorithm, as well as an overview of current ultrasound-based methods used for the detection and tracking of a needle in 2D or 3D US image feedback.

Ultrasound needle artifacts

We describe here several phenomena that are typically observed in ultrasound (US) images and that are specific to the presence of a needle in the field of view of the probe [START_REF] Reusz | Needlerelated ultrasound artifacts and their importance in anaesthetic practice[END_REF]. These phenomena creates artifacts that can limit the performances of a needle detection algorithm. An illustration of the different artifacts is shown in Fig. 3.6 and a picture of a needle observed in 3D US can be seen in Fig. 3.7.

Reflection:

The direction in which a US beam is reflected at an interface between two media with different densities depends on the angle of incidence of the wave on the interface. In some cases the beam can be reflected laterally such that the echo never returns to the transducer [START_REF] Reusz | Needlerelated ultrasound artifacts and their importance in anaesthetic practice[END_REF]. This effect reduces the visibility of the needle when the insertion direction is not perpendicular to the propagation of the US wave. This can be particularly visible with convex probes, for which the beam propagation direction is not the same at the different locations on the image, resulting in a variation of the intensity of the observed needle. This effect can be reduced by using echogenic needles with a surface coating that reflects the US beam in multiple directions. Special beam steering modes are also available on certain US probes, for which all elements of the transducer are activated with small delays to create a wave that propagates in a desired direction. This can be used to enhance the visibility of the needle when its orientation is known, as was done in [START_REF] Hatt | Enhanced needle localization in ultrasound using beam steering and learning-based segmentation[END_REF].

Reverberation/Comet tail artifact: The high difference between the density of the needle and the density of soft tissues induces a high reflection of the US wave at the interface. This occurs on both sides of the needle and in each direction, such that a part of the wave can be reverberated multiple times between the two walls of the needle. Multiple echos are subsequently sent back to the transducer with a delay depending on the distance between the walls and the number of reflections inside the needle. Since the image is reconstructed using the assumption that the distance from the probe is proportional to the time needed by the wave to come back to the transducer

Side lobes artifacts

Reverberation / Comet tail artifact Re¡ection attenuation (see (3.2)), the echos created by the reflections inside the needle are displayed as if they came from an interface located deeper after the needle. Hence a comet tail artifact can be observed in a cross-sectional view of the needle, due to the appearance of a bright trailing signal following the real position of the needle.

Beam width/Side lobes artifact: Due to the width of the US beam, it is possible that the needle is hit by several beams corresponding to different scan lines. This results in a needle that apparently spreads laterally and is larger than its real diameter. Similarly, the piezoelectric elements can emit parasitic side beams in addition to the main beam. The amplitude of the wave in the side beams is usually smaller than the amplitude of the main beam, which limits the influence that they have on the final image due to the attenuation in the tissues. However, strong reflectors like a needle may reflect the quasi-totality of the side beams, creating strong echos coming back to the transducer which are interpreted as returning from the main beam during the reconstruction of the image. This creates further lateral spread of the apparent position of the needle, as can be seen in Fig. 3.7.

Needle detection algorithms

Many image processing techniques have been proposed over the last decade to detect a needle in 2D or 3D ultrasound (US) images. Needle detection in 2D US is challenging because of the missing third dimension. The needle can be only partially visible and it is not always possible to ensure that it is entirely in the imaging plane. On the opposite, while the data acquired by a 3D US probe usually require some processing to be visualized in a comprehensible way by a human operator, they can easily by used directly by a computer process to detect the 3D shape of the needle. However it usually requires more computation due to the increased dimension of the image.

Tracking algorithms have also been proposed to find the position of the needle across a sequence of images. These algorithms usually use a detection algorithm that is applied on each newly acquired image. The result is enhanced by using a temporal filtering of the output of the detection algorithm, typically a Kalman filter, or a modification of the detection algorithm to take into account the position of the needle in the previous images.

In the following we present an overview of the general techniques that are used for needle tracking in 2D or 3D. Needle detection algorithms generally follow the same order of steps performed on the image:

• a pre-filtering of the image to enhance the needle visibility and remove some noise,

• a binarization of the image to select a set of potential points belonging to the needle,

• a shape fitting step to find the final localization of the needle.

Image pre-filtering: Smoothing of the image is often performed to filter out the speckle noise that is present in the image. This process also reduces the sharpness of the edges, which can be detrimental to find the boundaries of the needle. Median filtering is sometimes preferred to achieve noise smoothing while keeping a good definition of the edges.

In order to enhance the separation between the bright needle and the dark background, a modification of the pixel intensity levels can then be used. This can be achieve in many ways, such as histogram equalization [PZdW + 14] or exponential contrast enhancement [WRS + 16]. In the case where the needle is co-planar with the imaging plane and a guess of its orientation is known, a specific filter can be used to enhance the visibility of the linear structures with a given orientation. For example an edge-detector can be used in a given direction [OEC + 06]. Gabor filtering is also often used in 2D [START_REF] Kaya | Needle localization using gabor filtering in 2d ultrasound images[END_REF] or in 3D [PZdW + 14].

Image binarization: A threshold is applied to the pre-processed image to keep only the points that have a good probability of belonging to the needle. Otsu's thresholding method can be used to automatically find an optimal threshold that separates two classes of intensities [START_REF] Otsu | A threshold selection method from gray-level histograms[END_REF]. However this method can yield poor performances if the background itself has several distinct levels of intensity. This can occur on non-filtered images when low reflective structures, with a black appearance, are present within normal tissues, with a gray appearance. Therefore the value of the threshold is mostly tuned manually to a pre-defined value or such that a certain percentage of the total number of point is kept, which can introduce a great variability in the performances of the algorithms.

In [START_REF] Neubach | Ultrasound-guided robot for flexible needle steering[END_REF] needle tracking is performed using the difference between two successive US images. The resulting image presents a bright spot at the location where the needle tip progressed in the tissues, allowing to obtain a small set of pixels corresponding to the position of the needle tip after the thresholding process. However this method can only be used if the probe and the tissues are stationary, such that the motions in the images are only due to the needle.

Doppler US imaging is used to display the velocity of moving tissues instead of their density variations. Therefore, it can be used to naturally reduce the number of structures with the same intensities as the needle by applying fast vibrations to the needle. An active vibrator attached near the base of the needle is used in [START_REF] Adebar | 3d ultrasoundguided robotic needle steering in biological tissue[END_REF] to produce these vibrations. The rotation of the needle around its axis is used in [START_REF] Mignon | Using rotation for steerable needle detection in 3d color-doppler ultrasound images[END_REF] to create the same amount of motion all along the needle, avoiding the attenuation of the vibrations along the needle shaft that can be observed when using a vibrator at the needle base.

Needle shape fitting: Given a set of segmented needle points, a first decimation is often performed to remove obvious outliers. This typically involves morphological transformations, like a succession of erosions and dilatations, to remove the groups of pixels that are too small to possibly represent a needle. Fusion with Doppler US modality can also be used to get additional information in the needle location and remove outliers. Many methods can then be used to find the position of the needle depending on its configuration in the image.

When using a 2D US probe, the needle can first be perpendicular to the imaging plane, such that only a section of the shaft is visible and can be tracked. In [WRS + 16] the set of pixels corresponding to the measured needle section is used as the input for a Kalman filter to estimate the point of the set that represents the real center of the needle cross section.

The comet tail artifact exhibited by the needle can also be exploited to find the needle position. In [VAP + 14] and [AVP + 14] needle tracking is achieved by using the Hough transform applied to the pixel set to find the best line fitting the tail of the artifact. The center of the needle is then taken as the topmost extremity of the line and translated by a length corresponding to the radius of the needle. A similar process is used in [SRvdB + 16] where Fourier descriptors are used to find the center of the needle in the comet tail artifact.

In the case where the needle shaft is in the imaging plane of a 2D probe or the field of view of a 3D probe, line detection algorithms are used to find the best fit of the needle. A Hough transform is used in [OEC + 06] to find the best group of points that fits a linear shape and remove all of the other outliers. A polynomial fitting is then performed with the remaining points to find the final shape of the needle.

A now wide-spread method for line or polynomial fitting is the Random Sample Consensus (RANSAC) algorithm. The principle of the algorithm is to take a random sample of points and to build a polynomial fitting from this sample. The quality of the sample is assessed by the total number of points of the set that fits the obtained polynomial. The process is repeated many times and the fitting containing the maximum of points is taken as the result of the needle detection process. This algorithm is quite robust to outliers since a polynomial fitting from a sample containing outliers is likely to fit poorly to the real inliers. Such an algorithm was used in 2D US in [START_REF] Kaya | Needle localization using gabor filtering in 2d ultrasound images[END_REF] after a Gabor filtering and Otsu's automatic thresholding. It can also easilly be applied to 3D US, as done in [START_REF] Uherčík | Model fitting using ransac for surgical tool localization in 3-d ultrasound images[END_REF] after applying a simple threshold. Due to the stochastic nature of the algorithm, there is no guaranty that the final result of the algorithm contains only inliers, and inconsistent results can be obtained if the algorithm does not run for a sufficient amount of time. The algorithm can be made faster and more consistent by minimizing the number of outliers present in the initial set of points. This can for example be done by using a needle enhancing pre-filtering like a 3D Gabor filter [PZdW + 14]. For needle tracking in a US sequence, a temporal filtering can be used to filter the output of the RANSAC algorithm and to predict a region of interest in which the needle should lie. A Kalman filter was used for 3D needle tracking in [START_REF] Chatelain | Real-time needle detection and tracking using a visually servoed 3d ultrasound probe[END_REF] and improved with a mechanics-based model to predict the motion of the needle between two acquisitions in [START_REF] Mignon | Beveled-tip needlesteering using 3d ultrasound, mechanical-based kalman filter and curvilinear roi prediction[END_REF].

Direct approach: Some approaches use directly the image intensity to localize the needle without relying on a prior thresholding of the image. For example, projective methods consist in calculating the integral of the image intensity along a curve that represents an underlying model of the object sought in the image. The curve with the highest value for the integral is selected as the best representation. The most known projective method is the Hough transform, which uses straight lines as the model. The generalized Radon transform, using polynomials, can be used to track a needle in a 3D US volume, as performed in [START_REF] Neshat | Real-time parametric curved needle segmentation in 3d ultrasound images[END_REF]. Due to the high number of possible configurations for the model in the image, projective methods are highly computationally expensive, especially in 3D. In [OEC + 06] detection rays are first traced perpendicular to an estimation of the needle direction and an edge detector is run along each ray to only keep one pixel along each ray. This way the set of possible pixels belonging to the needle has a fixed and relatively small size. The Hough transform is then use to find a line approximation fitting the maximum of points. A polynomial fitting is finally performed to find the best shape of the needle.

The pixel intensities can also be used directly with template matching to provide a fast tracking of the needle tip, as is done in [START_REF] Kaya | Visual tracking of biopsy needles in 2d ultrasound images[END_REF]. An artificial neural network is used in [START_REF] Rocha | Flexible needles detection in ultrasound images using a multi-layer perceptron network[END_REF] to directly compute for each pixel in a region of interest the probability that this pixel belongs to the needle.

A particle filter is used in [START_REF] Chatelain | 3d ultrasoundguided robotic steering of a flexible needle via visual servoing[END_REF] to locate a needle in 3D US. Each particle consists in a 3D polynomial curve that is directly projected in the 3D US volume. The probability that a particle corresponds to the real needle in the volume is computed using the sum of the intensities of the voxels along the curve and an additional term for the tip detection.

In the following section, we present the needle tracking algorithm that we use in order to take into account the different points mentioned previously. We choose a direct approach to avoid the tuning of a threshold for a segmentation step, and we use a method that only considers a limited set of points in the image in order to keep a reduced computational complexity.

Intensity-based needle tracking

In this section we present the tracking algorithms that we designed to localize the 3D position of the needle shaft using stereo cameras or 3D ultrasound (US). Both algorithms use directly the intensity value of the pixels or voxels located near the previous position of the needle to find its new best position. Their local behavior allows for fast computations while using directly the intensities make them independent of the quality of a prior segmentation of potential points belonging to the needle. We first present the tracking using stereo cameras and then we focus on the design of the algorithm to track a needle in 3D US volumes.

Tracking with camera feedback

We present here the algorithm that we designed to track the 3D shape of a needle embedded in a translucent gelatin phantom using two stereo cameras. Since cameras are not clinically relevant to observe a needle embedded in real tissues, it will be used for the validation of other aspects of the insertion, such as the control of the needle trajectory. The experimental conditions are Figure 3.8: Illustration of the reconstruction of a 3D point from its position observed in two images acquired by two different cameras. The red dots represent the 2D position of the object seen in both images and the green dot is the estimation of the 3D position of the object. thus optimized such that the algorithm can provide an accurate and reliable measure of the needle position. A uniform background is provided such that the needle is clearly visible in the images. We describe in the following the different steps allowing the measure of the 3D position of the needle from the 2D images acquired by the cameras.

Camera registration: The two cameras are placed orthogonally to each other to provide a 3D feedback on the position of the needle and the phantom. The intrinsic parameters of each camera are first calibrated using the ViSP library [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] and a calibration grid made of circles. These parameters comprise the position of the optical center in the image, the ratio between the focal length and the size of a pixel as well as two parameters to correct for radial distortion of the image. Once these intrinsic parameters are known, a mapping can be determined between each object in the image and a corresponding line in 3D space on which the object is supposed to lie. The relative pose between the cameras (translation and rotation) is then calibrated using the same calibration grid viewed by both cameras [START_REF] Marchand | Pose estimation for augmented reality: A hands-on survey[END_REF]. Any object observed in both images at the same time can be mapped to two different lines in space using the intrinsic parameters. The position of the object in 3D space can then be estimated by finding the closest point to the two lines, as illustrated in Fig. 3.8. The 3D accuracy of this tracking system, calculated from the size of the pixels and the distance of the needle from the cameras, is approximately 0.25 mm around the location of the needle.

During the insertion procedure it can also be necessary to express the Needle detection in 2D images: We use a gradient-based tracking algorithm to track the needle seen in each image. The principle of the algorithm is depicted in Fig. 3.9. The needle shape is approximated in the image with a third order polynomial curve defined by four equi-spaced control points.

After the tracking has been initialized and a new image has been acquired, a line is drawn for each control point such that it is normal to the previous polynomial curve and passes by the control point. The two edges of the needle shaft are found as the points corresponding to the maximum and minimum values of the gradient along the normal line. The new position of the control point is taken as the center between these two edges. A Kalman filter is used to temporally smooth the position of each control point to avoid abrupt changes than may correspond to a bubble or another object with sharp edges near the control point.

A new polynomial curve is then computed from the new positions of the control points. An edge detection is finally performed along a line tangent to the extremity of the curve to find the new position of the needle tip.

3D needle reconstruction: After the tracking as been done in each image, two 2D polynomial curves are available, which correspond to the projection of the 3D needle on the images. Several points are sampled along one of the 2D curves and then matched to their corresponding point on the 2D curve in the second image using epipolar geometry, i.e. using the intrinsic parameters of the cameras and their relative pose to deduce the possible correspondences between different pixels in both images. A 3D point is then reconstructed from each pair of matching 2D points along the needle (see Fig. 3.8). Finally the 3D needle is reconstructed by fitting a 3D polynomial curve to the set of 3D points.

The new 3D needle curve is then projected back onto each image to initialize the tracking in the next images. This allows a further smoothing of the motion of the 2D curves in the images and provides a way to recover if one of the 2D tracking in the image partially fails.

Iterative tracking using ultrasound needle artifacts

We describe here the new tracking algorithm that we use to track a flexible needle in 3D ultrasound (US) volumes. We first mention different points that drove us toward the development of the algorithm and then we present the algorithm itself.

Needle artifacts:

The presence of a needle in the field of view of an US probe leads to strong echos reflected back to the transducer due to the high difference of density between the needle and the tissues. This results in a bright region observed in the reconstructed US volume. Therefore a majority of needle tracking algorithms are designed to find the location of the needle in the middle of a bright zone. However the bright signal observed around the needle can mostly be due to US artifacts, like reverberation artifacts or lateral resolution degradation due to side lobes and beam width, as was presented in section 3.3.1. The effects of lateral resolution degradation are usually symmetric, such that the needle can effectively be found in the center of the bright zone in the lateral direction. However in the beam propagation direction, only the first received echo corresponds to the first wall of the needle, while subsequent echos are either due to the second wall or reverberations inside the needle. Therefore the real position of the center of the needle is located just after the first echo and not in the center of the bright signal. Detecting the needle in the center of the bright zone would result in an erroneous estimation of the real position of the needle. For this reason the algorithm that we propose in the following is optimized to take into account such artifacts.

Segmentation: Some algorithms first perform a segmentation of the US volume to isolate the voxels that are likely to belong to the needle. A second algorithm, typically a Random Sample Consensus (RANSAC) algorithm, is then used to find among those voxels the largest set that fits the best a predefined geometrical model of the needle shape. Hence the performances of this second algorithm highly depend on the quality of the segmentation step, both in terms of accuracy and processing time. However the segmentation of the needle is also usually heavily dependent on a threshold that determines if a given voxel belongs or not to the needle. A too high value of the threshold may lead to ignore some parts of the needle that are less bright than the rest, due to shadowing from other structures or a too large angle of incidence with the US beam. On the contrary, with a low value of the threshold too many voxels may be included, belonging to bright structures, background noise or needle artifacts. In practice the best tuning of the threshold may depend on the actual content of the volume, which can change over time during a same operation. In order to avoid these issues, we use directly the intensity of the voxels without prior segmentation step.

Computation time: In order to perform real-time control, the tracking algorithm should be able to provide an estimation of the position of the needle than is not too outdated with the real position of the needle. The acquisition and reconstruction of a 3D US volume in Cartesian space already introduces a delay, such that any further delay should be reduced to the minimum. Time consuming algorithms, like projective algorithms, usually perform heavy computations on a large set of voxels. These approaches are usually optimized using parallelization to achieve good timing performances. However this require specialized hardware, which can increase the cost of a needle tracking system. On the opposite, local algorithms only consider a limited set of voxels in the vicinity of an initial guess of the needle position. This initial guess is then refined iteratively until the new position of the needle is found. The actual result of such methods depends on their initialization, however they can perform with great speed and accuracy when the initial guess is not too far from the real needle. By exploiting only the data in a small region, they also ignore most outliers present in the volume, like other linear structures that could be mistaken for a needle by a global detection algorithm. Therefore in the following we choose to use a local approach to perform needle tracking.

Iterative tracking using needle artifacts: In order to address the different points mentioned previously, we propose to detect the position of the shaft of the needle in 3D US using a local iterative algorithm that directly uses the voxels intensities and takes into account the artifacts that are specific to the needle.

The algorithm is initialized around a 3D polynomial curve that represents a prediction of the needle body position in the US volume. The curve is defined by N control points equi-spaced along the curve. Several polynomial curve candidates are then sampled all around the first one by displacing each of the control points by a given step in the directions normal to the needle. Five positions are thus generated for each control points, leading to a total of 5 N curve candidates. The best curve is selected among the candidates to maximize a cost function calculated from the voxels intensities. The algorithm is then repeated around the new selected curve, until no better curve can be found around the current best one. In the following we note c i the polynomial curve candidates, with i ∈ [[1, 5 N ]], and V (c i (l)) the intensity of the voxel at position c i (l), with l the curvilinear coordinate along the curve.

In order to take into account the different points mentioned previously, the cost function J(c i ) associated to a curve c i is defined as follows

J(c i ) = J 3 (c i ) L 0 (J 1 (l) + J 2 (l)) dl, (3.38)
where L is the length of the curve c i and J 1 , J 2 , J 3 are different sub-cost functions. Figure 3.10 provides an illustration of the different sub-cost functions used in the algorithm. J 1 is used to detect the first wall of the needle in the beam propagation direction and to place the curve at a distance corresponding to the radius of the needle under this edge:

J 1 (l) = - 0 -L d w(s) V (c i (l) + (s -r N ) d(l)) ds (3.39) + L d 0 w(s) V (c i (l) + (s -r N ) d(l)) ds,
where L d defines the amount of voxels taken to perform the integrals, w is a weighting function used to give more importance to the voxels near the center of the integration zone, d(l) ∈ R 3 is the beam propagation direction at needle point c i (l) and r N denotes the radius of the needle expressed in voxels. We used a triangular profile for w, defined such that

w(s) =      L d +s L 2 d if -L d < s < 0 L d -s L 2 d if 0 ≤ s < L d 0 otherwise . (3.40)
J 2 is used to promote the curves that are laterally centered in the bright zone, i.e. bright portions that spread in a normal direction to the US beam where L n defines the amount of voxels taken to perform the integral, and n(l) ∈ R 3 is a unit vector normal to the needle curve and beam propagation direction at the needle point c i (l) defined such that

J 2 (l) =
n(l) = d(l) × dc i dl (l) d(l) × dc i dl (l) . (3.42)
The parameters L d and L n can be tuned to set the number of voxels taken into account around the curve candidates. Low values can be used to decrease the computations but the algorithm becomes more sensitive to noise in the volume. On the contrary, high values increase the computation time but introduce a better filtering of the noise. A trade-off can be achieved

INTENSITY-BASED NEEDLE TRACKING

by choosing intermediate values corresponding to the expected dimensions of the cross section of the needle.

Finally J 3 is used to penalize curves with high curvatures that may result from fitting adjacent background noise

J 3 = + 1 L L 0 d 2 c i dl 2 (s) ds (3.43)
where is a parameter used to define a curvature threshold from which the curvatures are penalized.

Tip tracking: Once the curve has been laterally fitted, the location of the needle tip p t is sought in the alignment of the extremity of the best curve c best to maximize the following cost function

J 4 = 0 -Lt w(s) V p t + s dc best dl (L) ds (3.44) - Lt 0 w(s) V p t + s dc best dl (L) ds,
where L t defines the amount of voxels taken to perform the integral. The parameter L t can be tune similarly to L d and L n to find a trade-off between computational cost and sensitivity to noise.

Due to the local and iterative nature of the algorithm, its performances in terms of timing and detection accuracy depend on the quality of the initialization of the needle position. With a proper initialization, the algorithm can perform fast and fit the exact shape of the needle. This can for example be obtained by using a model-based estimation of the needle motion between two acquisitions of the US volume. The tracking and timing performances of the algorithm are evaluated in the next section.

Experimental validation

We propose to illustrate the performances of our needle tracking algorithm in 3D ultrasound (US) during the insertion of a needle. We compare the tracking result with an algorithm that uses the Random Sample Consensus (RANSAC) algorithm after an intensity-based binarization of the volume, in order to show the limitations that can appear with such algorithm.

Experimental conditions (setup in France): We use the wobbler probe and US station from BK Ultrasound to record a sequence of US volumes during the insertion of the Angiotech biopsy needle in a gelatin phantom. The needle is inserted using the Viper s650 and the US probe is held and The acquisition parameters of the US probe are set to acquire 31 frames during a sweeping motion with an angle of 1.46 • between successive frames. Due to the strong reflectivity of the walls of the container and the low attenuation in gelatin, a reverberation of the US wave occurs between the surface of the probe and the opposite wall. The acquisition depth is set to 15 cm, which is larger than the container, in order to remove the artifacts created by this reverberation from the region where the needle is in the volume. This results in the acquisition of one volume every 900 ms and a maximal resolution of 0.3 mm × 1 mm × 2 mm at the level of the needle, which is approximately 5 cm away from the probe. The spacial resolution of the post-scan volume is set to 0.3 mm in all directions and linear interpolation is used for the reconstruction. A focus length of 5 cm is set for the transducer to obtain a good effective resolution near the needle. The needle is inserted slowly at 1 mm.s -1 such that the needle position is only slightly different between two volumes.

Tracking algorithm: We compare our intensity-based tracking algorithm to the result obtained with a tracking using RANSAC algorithm. For our algorithm, we set the size of the integration regions L d , L n and L t to 10 voxels (see (3.39), (3.41) and (3.44)), corresponding to a distance of 3 mm around the needle. A manual initialization of both tracking algorithms is performed in the first volume after the needle as been inserted 1.5 cm in the phantom. The threshold for the volume binarization necessary for the RANSAC algorithm is chosen just after the initialization. The maximum intensity level along the needle is computed and the threshold is set to 80% of this value. The robustness of the RANSAC algorithm is increased by rejecting obvious outliers during the sampling process, which are identified if the length of the detected needle is lower than 90% of the length of the needle detected in the previous volume.

Results: Both algorithms can track the needle without failing in a sequence of 3D US volumes. However they yield different shapes of the tracked needle at the different steps of the insertion. We detail these differences in the following.

Limited needle intensity: Figure 3.12 shows two cross sections of a volume acquired near the beginning of one insertion. Due to the location and orientation of the needle with respect to the probe, a great part of the US beam reflected by the needle shaft does not return to the transducer, resulting in a low intensity along the needle. On the contrary, the needle tip is more visible and some strong reflections also occur near the surface. Hence, after applying a threshold to the image for the RANSAC algorithm, only the tip and the artifacts due to the insertion point remains. The needle tip can still be found thank to the rejection of short fitting curves in the RANSAC algorithm, without which the best linear fit would be the artifact in this case. However the result does still overfit the artifact, leading to a global tracking that does not correspond to the real shape of the needle. On the other hand, our algorithm can accurately fit the shape of the needle in spite of the low intensity along the needle shaft.

This shows that using a threshold to binarize the volume does not allow an adaptation to the variations of intensity along the needle. On the opposite, taking into account all levels of intensities allows exploiting all the information available on the edge of the needle, leading to a better tracking. Let us now consider the cases where higher intensities are available along the needle shaft and not only at the needle tip.

Artifact fitting: Figure 3.13 shows three cross sections of a volume acquired in the middle of the insertion process. This time a part of the needle is almost normal to the beam propagation direction, such that a strong echo is reflected and results in a clearly visible bright region. Reverberation and side lobes artifacts are clearly visible in this case. The algorithm based on RANSAC tends to center in the middle of the bright region, which mainly contains reverberation artifacts. The resulting tracking is thus shifted with respect to the real position of the needle shaft. On the contrary our algorithm can fit to the first echo produced by the needle.

Conclusion:

These experiments have shown that our intensity-based tracking algorithm allows taking into account needle artifacts, created by reverberation or beam width, to accurately detect the position of the needle body in 3D US volumes. Using directly the voxels intensities allows adapting to variations of intensities along the needle shaft. This point is important for real applications since the needle intensity may vary due to different phenomena, such as reflection outside of the transducer or shadowing from other Figure 3.12: Tracking of the needle at the beginning of the insertion. The needle tracked using the proposed algorithm is represented in green and the needle tracked using the RANSAC algorithm is represented in red. Due to the large incidence angle with the ultrasound beam, the intensity along the needle shaft is reduced. Thresholding the image for the RANSAC algorithm yield only the needle tip and the strong reflections near the surface, leading to inaccurate needle shaft detection. On the contrary, taking all voxels into account leads to a better detection of the edges of the needle. Figure 3.13: Tracking of the needle in the middle of the insertion. The needle tracked using the proposed algorithm is represented in green and the needle tracked using the RANSAC algorithm is represented in red. Reverberation artifact is visible along the needle shaft in the beam direction (approximately x axis) resulting in a comet tail that can be seen in the needle cross section view (xz view). Side lobes artifacts normal to this direction can also be seen on each side of the needle (along the z axis). Some parts of these artifacts are included in the binarized volume after thresholding, resulting in a biased tracking with the RANSAC algorithm. On the contrary, the tracking taking artifacts into account fits the first echo and ignores the reverberation artifact.

structures. Therefore, this tracking algorithm will be used in the following for all experiments performed under 3D US feedback.

Nevertheless, we tested the tracking using a slow insertion speed such that the needle motion was small between two acquisitions. The local tracking could then perform smoothly. In practice it is possible that motions with greater amplitude occur between two acquisitions, either due to a faster manipulation of the needle base or due to some movements of the tissues induced by physiological motions of the patient. The first point can be addressed by using a model that can predict the new position of the needle after a given motion has been applied to its base, like the model that we proposed in the previous chapter 2. The second point, however, requires to estimate the motions of the tissues, which will be the focus of the next section.

Tissue motion estimation

During needle insertion, physiological motions of the patient, like breathing, can induce a displacement of the tissues around the needle. This can modify the needle shape and the future trajectory of the needle tip. The effect of lateral tissue motions is all the more important when using flexible needles. Such needles indeed tend to follow the motions of the tissues without applying a lot of resistance. The modification of the future trajectory is also amplified when the part of the needle that is outside of the tissue is long, mainly at the beginning of the insertion. Therefore the interaction model needs to be updated online in order to account for such tissue motions and be able to provide a good estimation of the current state of the insertion.

In this section we present the method that we propose and have validated to update the model. The position of the model of the tissues presented in previous chapter (section 2.4.2) is estimated using an unscented Kalman filter (UKF). We first give a general presentation of Bayesian filtering and the formulations of particle filters and UKF. In a second part we provide more details to explain how we adapted the UKF to different kinds of available measurements including needle position feedback, provided by visual or electromagnetic tracking, and force feedback.

Multimodal estimation

Bayesian filtering

In this section we present and develop the general principles of Bayesian filtering that leads to the design of the unscented Kalman filter (UKF) and particle filter (PF). In the following sections and chapters, the UKF will be used for state estimation and applied to the case of needle-tissue interaction modeling.

TISSUE MOTION ESTIMATION

System modeling: Bayesian filtering is a general approach used to estimate the state of a system given some observations of this system. The first step is to provide a model of the evolution of the state of the system over time. Let us consider a system that can be fully parameterized at each instant using a state vector x ∈ R Nx containing N x state variables. The system can also be controlled using an input vector u ∈ R Nu containing N u components. The evolution of the system can generally be modeled with a state equation such that

x k+1 = f k (x k , u k , w k ), (3.45)
where k represents the time index, w k ∈ R Nw is a process noise of dimension

N w with covariance matrix Q k ∈ R Nw×Nw and f k : R Nx ×R Nu ×R Nw → R Nx
is a function to model the deterministic behavior of the system. Let y ∈ R Ny be a vector of N y measures on the system such that

y k = h k (x k , u k , ν k ), (3.46)
where

ν k ∈ R Nν is a measurement noise of dimension N ν with covariance matrix R k ∈ R Nν ×Nν and h k : R Nx × R Nu × R Nν → R Ny is a function representing the deterministic measurement model.
General principles: Bayesian filtering consists in estimating the probability density function (pdf) p(x k |y k , . . . , y 0 ) of the current state knowing the current and past measurements. In the following we slightly develop the computations that are used to provide a recursive estimation of p(x k |y k , . . . , y 0 ). It can be shown using Bayes law that we have the following relationship:

p(x k |y k , . . . , y 0 ) = p(y k |x k , y k-1 , . . . , y 0 ) p(x k |y k-1 , . . . , y 0 ) p(y k |y k-1 , . . . , y 0 ) , (3.47)
where p(y k |x k , y k-1 , . . . , y 0 ) is the pdf of the current measure knowing the current state of the system and the past measures, p(x k |y k-1 , . . . , y 0 ) is the pdf of the current state of the system knowing the past measures and p(y k |y k-1 , . . . , y 0 ) is the pdf of the current measure knowing the past measures.

First it can be seen that the denominator p(y k |y k-1 , . . . , y 0 ) does not depend on x k and is thus equivalent to a scaling factor for p(x k |y k , . . . , y 0 ). Since the integral of a pdf is always equal to 1, it is sufficient to compute and normalize the numerator, so that this scaling factor does not need to be computed and can be dropped. In addition, in order to simplify the derivation of the recursive filter, it is assumed that the system follows a first order Markov process, i.e. the state x k of the system at time k only depends on the previous state x k-1 and is independent of the other states before that It can also be noted that p(x k |y k-1 , . . . , y 0 ) can be further developed using the chain rule:

p(x k |y k-1 , . . . , y 0 ) = p(x k |x k-1 )p(x k-1 |y k-1 , . . . , y 0 )dx k-1 , (3.49)
where p(x k |x k-1 ) is the pdf of the current state knowing the previous state of the system and p(x k-1 |y k-1 , . . . , y 0 ) is the pdf of the previous state knowing the past measures. Finally, we get the recursive formula

p(x k |y k , . . . , y 0 ) ∝ p(y k |x k )p(x k |y k-1 , . . . , y 0 ) (3.50) ∝ p(y k |x k ) p(x k |x k-1 )p(x k-1 |y k-1 , . . . , y 0 )dx k-1 . (3.51)
An graphical illustration of this equation is provided in Fig. 3.14. In practice p(x k-1 |y k-1 , . . . , y 0 ) is known from the previous step of the recursive method, p(x k |x k-1 ) can be estimated using the evolution model (3.45) and p(y k |x k ) can be estimated using the the measurement model (3.46). Hence most Bayesian filters proceed in two steps: a prediction step, where a prediction of the state is made based on the previous estimate, i.e. p(x k |y k-1 , . . . , y 0 ) is computed using (3.49), and an update step, where the new measure is integrated to correct the prediction, i.e. p(x k |y k , . . . , y 0 ) is computed using (3.50).

TISSUE MOTION ESTIMATION Figure 3.15: Illustration of the pdf approximations used by Kalman filters (KF) and particle filters (PF).

Implementations: There exists many families of Bayesian filters that use different methods to get the estimations of the different pdfs and perform the prediction and update steps. For an overview of Bayesian filters we invite the reader to refer to [START_REF] Van Der Merwe | The unscented particle filter[END_REF] or [START_REF] Chen | Bayesian filtering: From kalman filters to particle filters, and beyond[END_REF].

The family of the particle filters uses a finite set of samples to approximate the pdf. This allows a good estimation of the pdfs but requires more computational resources, especially for a high dimensional state space.

The family of the Kalman filters (KFs) uses the Gaussian approximation, i.e. all the pdfs are Gaussian. This greatly reduces the computations but may lead to approximations when the real pdfs are highly non-Gaussian. Figure 3.15 shows an illustration of these different approximations.

In the following we briefly focus on particle filtering before detailing more thoroughly the Kalman filters.

Particle filter

The principle of the particle filters (PFs) is to use a large set of N p weighted samples X i , called particles, to approximate the different pdfs. The weights w i associated to each particle are a representation of the likelihood of the particle and are defined such that

Np i=1 w i = 1. A pdf g(x) of a random variable x is thus approximated as g(x) ≈ Np i=1 w i δ (x -X i ) (3.52)
where δ is the Dirac delta function. Using this approximation, the pdfs in (3.51) are reduced to finite sums. The main advantage of the PF is that it can be used with non-linear systems as well as non-Gaussian pdfs. However its performance depends on the number of particles used in the approximations. A high number of particles is usually required to obtain a good accuracy, especially when considering high dimensional state spaces, which increases its required computational load. Many variants of the PF exist depending on the method used to sample and update the particles [START_REF] Chen | Bayesian filtering: From kalman filters to particle filters, and beyond[END_REF].

On the contrary, Kalman filters offer a reduced complexity and are deterministic since they do not rely on a random sampling process. Therefore we will use these kind of filters in the following.

Kalman filters

Let us develop the case of the KFs a bit further. Under the Gaussian assumption, each pdf can be entirely characterized using only their mean µ and covariance matrix P , such that they take the form

p(x) = 1 (2π) Nx |P | e -1 2 (x-µ) T P -1 (x-µ) , (3.53)
with |P | the determinant of P and . T the transpose operator. An estimate of the state at the end of each step can directly be built using the mean of the state pdf and the covariance matrix gives the uncertainty on this estimate. In the following we note xk|k-1 and P x,k|k-1 the mean and covariance matrix, respectively, of the pdf of the state at the end of the prediction step, and xk and P x,k the same at the end of the update step. We also introduce the prediction of the measures at the end of the prediction step ŷk .

It can be shown that the update step can be reduced to

xk = xk|k-1 + K k (y k -ŷk ), (3.54) P x,k = P x,k|k-1 -K k P ỹ,k K T k , (3.55) K k = P xy,k P -1 ỹ,k , (3.56)
where K k ∈ R Nx×Ny is called the Kalman gain, P xy,k ∈ R Nx×Ny is the covariance matrix between x k and y k , and P ỹ,k ∈ R Ny×Ny is the covariance of the innovation ỹk = y k -ŷk . Different versions of KFs can be derived depending on the method used to propagate the pdfs through the evolution and observation equations. For completeness we briefly describe the most known classical KF and extended Kalman filter (EKF) before detailing the UKF.

Kalman filter and extended Kalman filter: For both KF and EKF, the propagations of the pdfs are done by directly propagating the means through the system equations, (3.45) and (3.46), and the covariance matrices through a linearized version of the equations. The prediction step is thus

TISSUE MOTION ESTIMATION computed according to

xk|k-1 = f k (x k-1 , u k-1 , 0), (3.57) P x,k|k-1 = F k-1 P x,k-1 F T k-1 , + W k-1 Q k-1 W T k-1 (3.58) ŷk = h k (x k|k-1 , u k , 0), (3.59)
where

F k = ∂f k ∂x x=x k ∈ R Nx×Nx , (3.60) W k = ∂f k ∂w x=x k ∈ R Nx×Nw . (3.61)
The update step is performed as stated previously in (3.54) and (3.55) with the values

P xy,k = P x,k|k-1 H T k , (3.62) P ỹ,k = H k P x,k|k-1 H T k + G k R k G T k , (3.63)
where

H k = ∂h k ∂x x=x k|k-1 ∈ R Ny×Nx , (3.64) G k = ∂h k ∂ν x=x k|k-1 ∈ R Ny×Nν . (3.65)
The difference between the KF and EKF is that the KF makes the additional assumptions that the system is linear, while the EKF can be used with non-linear systems. This way no linearization is required for the KF, which reduces the computational complexity and makes it easy to implement. In this case the system equations become

x k+1 = F k x k + B k u k + W k w k , (3.66) y k = H k x k + D k u k + G k ν k , (3.67) with B k ∈ R Nx×Nu and D k ∈ R Ny×Nu .
Unscented Kalman filter: The UKF proposed by Julier et al. [START_REF] Julier | A new extension of the kalman filter to nonlinear systems[END_REF] is a sample-based KF hence approaching from a PF. It uses a small number of weighted state samples, called sigma points, to approximate the Gaussian pdfs. The propagation of the pdfs through the system is done by propagating the sigma points in the system equations. The advantage of this method is that it does not linearize the equations around one point but instead propagates the sigma points through the non-linearities. This way the propagation can be achieved with a higher order of approximation than with the EKF in the case of a highly non-linear system [WVDM00] while also being less computationally demanding than PF.

In the case of a numerical model for which the linearization in the EKF can not be done analytically, the UKF requires similar computations as the EKF. Therefore, due to its better performances and simplicity, we will use the UKF to update our numerical interaction model instead of the other filters presented previously. We develop its principle a bit further in the following.

Augmented state: Usually, the process and observation noises, w and ν, are incorporated with the state x in an augmented state x a ∈ R Nx+Nw+Nν such that

x a =   x w ν   , P a x =   P x P xw P xν P xw Q P wν P xν P wν R   (3.68)
with P xw ∈ R Nx×Nw the covariance between the state and process noise, P xν ∈ R Nx×Nν the covariance between the state and the measurement noise and P wν ∈ R Nw×Nν the covariance between the process noise and the measurement noise. Under this form, the UKF allows taking into account non-linear incorporation of correlated noises. For simplicity of notation and computation, we assume in the following that the process and measurement noises w and ν are independent additive noises. This way P xw = 0, P xν = 0, P wν = 0 and the system equations take the form

x k+1 = f k (x k , u k ) + w k , (3.69) y k = h k (x k , u k ) + ν k , (3.70)
This simplification allows us to follow the UKF steps using only the state x instead of the augmented state x a .

Prediction and update steps: At each iteration, a set of

2N x + 1 sigma points X i , i ∈ [[0, 2N x ]]
, is sampled from the current state pdf according to the weighted unscented transform, so that

   X 0 = xk-1 , X i = xk-1 + ( √ N x αP x ) i , i = 1, . . . , N x , X i = xk-1 -( √ N x αP x ) i-Nx , i = N x + 1, . . . , 2N x , (3.71)
where α is a positive scaling factor than is used to control the spread of the sigma points and () i denotes the i th column of a matrix. Using a large value for α leads to wide spread sigma points and a small value leads to sigma points close to each other. Tuning this parameter may be difficult as it should depend on the shape of the non-linearity that is encountered. Close sigma points may be equivalent to a linearization of the non-linearity while spread sigma points may be too far from the non-linearity of interest, which may lead to a reduced quality of the filtering in both cases. The prediction step is performed by propagating each sigma point through the evolution equation (3.69):

X i ← f k-1 (X i , u k-1 ) i = 0, . . . , 2N x .
(3.72) The mean xk|k-1 and covariance matrix P x,k|k-1 of the Gaussian pdf associated to the new propagated set can then be computed using weighted sums along the new propagated sigma points:

xk|k-1 = 2Nx i=0 W (m) i X i , (3.73) P x,k|k-1 = Q + 2Nx i=0 W (c) i (X i -xk|k-1 )(X i -xk|k-1 ) T , (3.74) with W (m) 0 = (α 2 -1) α 2 , (3.75) W (c) 0 = (α 2 -1)
α 2 + 3 -α 2 , (3.76) W (m) i = W (c) i = 1 4α 2 , i = 1, . . . , 2N x . (3.77)
For the update step, a corresponding estimate of the measures Y i is then associated to each sigma point using the measure equation (3.70):

Y i = h k (X i , u k ) , i = 1, . . . , 2N x . (3.78)
The standard update step ((3.54)-(3.56)) is finally performed to obtain the final estimate of the new state mean and covariance. The different terms CHAPTER 3. NEEDLE LOCALIZATION USING ULTRASOUND are estimated as weighted sums along the sigma points:

ŷk = 2Nx i=0 W (m) i Y i ,
(3.79)

P xy,k = 2Nx i=0 W (c) i (X i -xk|k+1 )(Y i -ŷk ) T , (3.80) P ỹ, k = R + 2Nx i=0 W (c) i (Y i -ŷk )(Y i -ŷk ) T . (3.81)
An illustration of the different steps of the UKF is provided in Fig. 3.16.

Discussion: As a side remark, it can be noted that all the operations performed in the KFs assumes that the variables lie in a vector space. In the case where one of the variables lies in a manifold that does not reduce to a vector space, the vector operations, such as addition or multiplication by a matrix, lose their signification. The Gaussian pdfs are also harder to define on manifolds. This can typically be the case when considering orientations in the state space or measurements space. In that case we use the manifold version of the KFs as described for the UKF by Hauberg et al. [START_REF] Hauberg | Unscented kalman filtering on riemannian manifolds[END_REF]. This method basically consists in mapping the variables (sigma points or their associated measure estimates) to a tangent space at some point of the manifold using the logarithm map. This way all the linear operations developed previously can be used on this tangent space, which is a vector space. Note also that the covariance matrices only make sense on the tangent space. Once the calculations have been performed, the resulting estimates of the state or measures can be mapped again on the manifold using the exponential map. At each prediction step the tangent space of the state manifold is taken at the current state estimate xk , corresponding to the center sigma point X 0 . The remaining sigma points are sampled in this tangent space according to (3.71). Similarly, at each update step the tangent space of the measure manifold is taken at the measure estimate of the center sigma point Y 0 = h k (X 0 , u k ). The measures associated to each sigma point are then all mapped to this tangent space. The covariance matrices can then be computed using (3.80) and (3.81) by replacing in the equations the sigma points and measure estimates by their corresponding mapping on the tangent spaces. An illustration of the logarithm and exponential map as well as the different steps of the UKF on manifold spaces can be found in Fig. 3.17. Now that the general formulation of the UKF has been presented, next section develops how we make use of it to update the state of our needletissue interaction model. Figure 3.17: Illustration of the unscented Kalman filter on manifolds.

Tissue motion estimation using unscented Kalman filter

In this section we present how we use the unscented Kalman filter (UKF) to estimate the tissue motions and update our needle-tissue interaction model presented in section 2.4.2. We will consider two kinds of measurements: measurements on the geometry of the needle, such as position or direction of some point of the needle shaft, and measurements of the force and torque exerted at the base of the needle. The method is described in such a way that it is independent of the method actually used in practice to provide the measurements. Position and direction feedback can for example be provided by an electromagnetic (EM) tracker placed somewhere inside the needle or through shape reconstruction using fiber Bragg grating [PED + 10]. It can also be provided by a needle detection algorithm that runs on some visual feedback; the visual feedback itself can be of various nature, as for example a sequence of 2D or 3D images provided by cameras [START_REF] Bernardes | Robot-assisted automatic insertion of steerable needles with closed-loop imaging feedback and intraoperative trajectory replanning[END_REF], ultrasound [START_REF] Kaya | Visual tracking of biopsy needles in 2d ultrasound images[END_REF], computerized tomography [HGG + 13] or magnetic resonance imaging [PvKL + 15]. We do not consider the case where the position of the tissues is directly provided, for example by using an EM tracker or a visual marker placed on the tissue surface. Although the method could also be used with such measures, it poses additional issues that will be observed and discussed later in section 3.6.2.

Evolution equation

Let us define the state of the UKF as the position x ∈ R 3 of the tissues in the two-body model. We take this state as the position of the extremity of the tissue spline near the tissue surface (entry point) and expressed in the world frame {F w }, as illustrated in Fig. 3.18. We consider here that the tissue spline can not deform, such that the state x is then sufficient to characterize the whole motion of the tissue spline.

In the case where prior information is known on the tissue motions, this can be included in the evolution model by choosing an adequate function f k in (3.45). For example a periodic model of breathing motion [HMB + 10] can be used when needle insertion is performed near the lungs and the patient is placed under artificial breathing, leading to

x k = a + b cos 2n πt k T + φ , (3.82)
where T is the period of the motion, a ∈ R 3 is the initial position, b ∈ R 3 is the amplitude of the motion, φ ∈ R is the phase of the motion, n ∈ N is a coefficient used to tune the shape of the periodic motion and t k is the time. Using a model for tissue motions has the advantage that the process noise in the filter can be tuned with lower values of uncertainties in the covariance matrix, leading to an overall better smoothing of the measures. It can also be used to provide a prediction of the future position of the tissues. However, if the model does not fit the real motion, it may lead to poor filtering performances. In most situations, the exact motion of the tissues is not known and additional parameters to estimate need to be added to the state, such as the motion amplitude b, the period T or the phase φ. This, however, adds a layer of complexity to the model and can induce some 3.5. TISSUE MOTION ESTIMATION observability issues if the number of measurements is not increased as well.

In clinical practice, patients are rarely placed under artificial breathing or even general anesthesia for needle insertion procedures such a biopsies. Breathing motion can then be hard to model perfectly since it may have amplitude or frequency varying over time. It may also happen that the patients suddenly hold their breath or simply move in a way that is not expected by the model. In this case the prediction can be far from the reality and may cause the state estimation to diverge.

In order to take into consideration the previous remarks and be able to account for any kind of possible motions, we choose a simple random walk model. This offers great flexibility but at the expense of reduced prediction capabilities on the tissue motions. The corresponding evolution equation can be written as

x k+1 = x k + w k . (3.83)
The advantage of this form is that the equation is linear and the noise is additive. This way it is not required to use the unscented transform to perform the prediction step, which reduces to xk|k-1 = xk-1 , (3.84)

P x,k|k-1 = P x,k-1 + Q k-1 . (3.85)
The sigma points can then be sampled using xk|k-1 and P x,k|k-1 for the update step that we describe in the following.

Measure equation

One advantage of the UKF is that we can use our interaction model to provide a numerical way to compute the measure function h k without analytic formulation. We consider the case where the needle is constantly held by a needle holder that provides a pose feedback of its end effector thanks to mechanical odometry. The pose of the needle base in the model is thus regularly updated using this feedback during the insertion. This way, even without tissue motions, it is possible that the shape of the needle changes. Therefore the function h k relating the estimated state to the measurements can greatly vary between two successive update steps and provides some prediction of the measures.

Needle position: Let us first consider the case where the measurements consist in a set of points belonging to the needle. Let us define a set of M points p j , j ∈ [[1, M ]], located at some given curvilinear coordinates l j on the needle. In that case the measure vector can be written as

y =    p 1 . . . p M    . (3.86)
From the model of the needle, the estimates pj of the measured needle points can be computed according to pj = c N (l j ), (3.87)

where we recall that c N is the spline curve representing the needle in the model. Note that it is possible to change the dimension of the measure vector y and the curvilinear coordinates l j depending on the measures that are available. For example if a needle tracking algorithm is used, points can be added when and where the needle is clearly visible in the image, while fewer points may be available when and where the needle is almost not visible. The dimensions of the measurement noise vector ν k and its covariance matrix R k will also vary accordingly.

Needle direction: In some cases the direction of the body of the needle at some given curvilinear coordinates l d can also be measured. This is typically the case when using a 5 degrees of freedom EM tracker embedded in the tip of the needle. In that case the measure vector can be written as

y = d =   d x d y d z   , (3.88)
where d ∈ S 2 is a unit vector tangent to the body of the needle at the curvilinear coordinates l d and S 2 denote the unity sphere in R 3 . From the model of the needle, the estimates of the needle body direction at curvilinear coordinate l d can be computed according to

d = dc N (l) dl l=l d . (3.89)
In that case, since S 2 is not a vector space, we need to use the version of the UKF on manifold that was discussed in section 3.5.1.3. The tangent space of S 2 is taken at the measure estimate Y 0 associated to the center sigma point. In this particular case the logarithm map of a measure point Y i is the angle-axis rotation vector θu representing the rotation between Y 0 and this measure point. This can be computed using

Log Y 0 (Y i ) = θu, (3.90) with u = Y 0 × Y i Y 0 × Y i , (3.91) θ = atan2( Y 0 × Y i , Y 0 .Y i ), (3.92)
where × denotes the cross product between two vectors, u is the axis of the rotation and θ is the angle between the two vectors Y 0 and Y i . The exponential map of an angle-axis rotation vector θu in the tangent space is obtained by rotating Y 0 according to this rotation vector, such that

Exp Y 0 (θu) = cos(θ)Y 0 + sin(θ)u × Y 0 .
(3.93)

TISSUE MOTION ESTIMATION

Efforts at the needle base: Let us now consider the measures of the force and the torque exerted at the needle base. Since our needle model does not take into account any axial compression or torsion, it can not be used to provide estimates of the axial force and torque exerted on the base. So we only consider the measures of the lateral forces and torques, which are sufficient to estimate the lateral motions of the tissues. The corresponding measure vector can be written as

y = f b t b . (3.94)
where f b ∈ R 2 is the lateral force exerted at the base of the needle and t b ∈ R 2 is the lateral torque exerted at the base of the needle. From the model of the needle, the estimates of the force f b and torque tb can be computed according to the Bernoulli equations

f b = EI d 3 c N (l) dl 3 l=0 , (3.95) tb = EI d 2 c N (l) dl 2 l=0 × z, ( 3.96)
where we recall that E is the Young's modulus of the needle, I is the second moment of area of a section of the needle, c N is the spline curve representing the needle in the model and z is the axis of the needle base.

Update step: A complete measure vector first needs to be chosen as a combination of the different measurements defined previously, as for example a vector stacking the force measures provided by a force sensor and the position and direction measures provided by an electromagnetic tracker.

Let us now describe how is performed the update step at each new acquisition of the measures. The state of the whole needle-tissue model is first saved at the moment of the acquisition. The sigma points X i are then sampled using (3.71) around the estimate of tissue position obtained at the prediction step. A new needle-tissue model is then generated for each sigma point and the position of each spline c T representing the position of the tissues is modified according to the sigma point X i . The new needle shape of each model is then computed and the estimates of the measures Y i can be generated from the model as defined previously in (3.87), (3.89), (3.95) or (3.96).

Since the actual spread of the sigma points depends on the covariance P x,k|k-1 , it can happen that a high uncertainty leads to unfeasible states. For example if the distance between the current state estimate and one of the sigma points is greater than the length of the needle, it is highly probable that the model of the needle corresponding to this sigma point can not interact with the model of the tissues anymore. Such sigma point should thus be rejected to avoid failure of the computation of the model or at least avoid irrelevant estimates of the measures. Therefore the value of α is tuned at each update step to avoid such numerical issues (see (3.71)). A small value α = 10 -3 is chosen as the default, as is typically done in a lot of works using the UKF [START_REF] Wan | The unscented kalman filter for nonlinear estimation[END_REF]. We then adaptively reduce the value of α when needed such that the sigma points do not spread further than 1 mm from the current estimated position of the tissues.

The new state estimate and state covariance can finally be updated according to the update step equations defined previously ((3.54)-(3.56) and (3.79)-(3.81)). Finally, the position of the whole tissue spline in the model is updated according to the value of xk computed by (3.54). Now that we have described a method to estimate the position of the tissues from measures provided on the needle, we propose in the following to use this method and assess its performances in different experiments.

Tissue update validation

In this section we present different experimental scenarios to evaluate the performances of our tissue motion estimation algorithm using the unscented Kalman filter. We first present the results obtained using the effort feedback provided by a force sensor and the position feedback on the needle tip provided by an electromagnetic tracker. Then we consider the case of position feedback on the needle shaft provided by cameras. Finally, we estimate the position of the tissues using the position feedback provided by a 3D ultrasound probe and use this estimation to improve the robustness of the needle tracking algorithm.

Update from force and position feedback

We consider in this section the update of the model using the force and torque feedback on the needle base as well as the position and direction feedback on the needle tip.

Experimental conditions (setup in the Netherlands): The setup used in these experiments is depicted in Fig. 3.19. We use the needle insertion device (NID) attached to the UR3 robot. The Aurora biopsy needle with the embedded electromagnetic (EM) tracker is placed inside the NID and inserted in a gelatin phantom. The UR5 robot is used to apply a known motion to the phantom. The ATI force torque sensor is used to measure the interaction efforts exerted at the base of the needle and the Aurora EM tracker is used to measure the position and direction of the tip of the needle.

We use the two-body model presented in section 2.4.1 with polynomial needle segments of order r = 3 to represent the part of the needle that is Registration: Registration of the position of the EM tracking system in the frame of the UR3 robot is performed before the insertions. The needle is moved at different positions and two sets of positions are recorded, one given by the UR3 odometry and one given by the EM tracker. Point cloud matching between the two sets is then used to find the pose of the EM tracking system in the frame of the UR3.

The force torque sensor is used to measure the interaction efforts between the needle and the tissues. Since the sensor is mounted between the UR3 robot arm and the NID, it also measures the effect of the gravity due to the mass of the NID. Therefore the effect of gravity must be removed from the measures in addition to the sensor natural biases to obtain the desired measures. Note that we only apply small velocities and accelerations to the NID during our experiments and for this reason we choose to ignore the effects of inertia on the force and torque measurements. The details of the force sensor registration can be found in Appendix A.

Experimental scenario: The force and EM data were acquired during the experiments on motion compensation that will be presented later in the thesis. In this section we only take into account the different measurements that were acquired during those experiments and we do not focus on the actual control of the needle that was performed. During those experiments, a known motion was applied to the phantom with the UR5 while the needle was inserted at constant speed with the NID. The UR3 was controlled to apply a lateral motion to the whole NID to avoid tearing the gelatin or breaking the needle.

Update method: The length of the needle model is updated during the insertion to correspond to the real length of the part of the needle that is outside the NID, measured from the full length of the needle and the current translation of the NID. The pose of the simulated needle base is updated using the pose of the UR3 and the rotation of the needle inside the NID.

The position of the modeled tissues is estimated using our update algorithm based on the unscented Kalman filter (UKF) presented in section 3.5.2. In order to determine the contribution of each component, in the following we consider three update cases: one using only the force and torque feedback at the needle base, one using only the position and orientation feedback of the needle tip and the last one using all the measures. In each case the different measures are stacked in one common measure vector that is then used in the UKF. The estimations for each kind of measures are computed as described in previous section 3.5.2.2, i.e. using (3.94) to (3.96) for the force and torque feedback, (3.86) and (3.87) for the position feedback and (3.88) to (3.93) for the orientation feedback.

For each method, we consider that the measurements are independent, such that the measurement noise covariance matrix R in the UKF (used in (3.81)) is set as a diagonal matrix. The value of each diagonal element is set depending on the type of measure: (0.7) 2 mm 2 for the tip position, (2) 2 ( • ) 2 for the tip orientation, (0.2) 2 N 2 for the force and (25) 2 (mN.m) 2 for the torque. These values are chosen empirically, based on the sensors accuracy and the way they are implemented in the setup. The process noise covariance matrix Q (used in (3.74)) is also set as a diagonal matrix with diagonal elements set to (0.2) 2 mm 2 .

Results on the filtering of the measures: We first compare the difference between the measured quantities and their values estimated using the model updated by the UKF. An example of tip positions as well as the position and orientation estimation errors obtained during one of the experiments is shown in Fig. 3.20. The position and orientation measures obtained from the EM tracking system are considered as the ground-truth for these experiments. We can see that the tip position and orientation are better estimated when using only the tip measurements, while using only the force and torque feedback tends to introduce a drift in the estimation that increases with the depth of the needle tip in the gelatin. This could be expected because of the flexible nature of the needle. Near the tissue surface the pose of the needle base has a great influence on the needle shape. On the other hand, the shape of a flexible needle is progressively determined by its interaction with the tissues as it is deeper inserted. The interaction force near the tip of the needle tends to be damped by the tissues and have little influence on the force measured at the needle base. Then, the more the needle is inserted, the less information about the needle tip is provided by the force and torque measured at the needle base.

Force and torque measures are respectively shown in Fig. 3.21 and 3.22. The measures obtained from the force sensor are considered as the groundtruth for these experiments. We can observe that even when using only the force and torque feedback, the estimated measures of the torque does not seem to fit the real measures as well as expected. This can be explained by the low value of the torque measures compared to the value of the variance that was set in the UKF, such that the torque is almost not taken into account for the estimation in this case. The low value of the measures can be explained by the experimental conditions. Indeed, the needle can slide in and out the NID to modify its effective length, meaning that the effective base of the needle is not fixed to the NID. This introduces a play between the needle and the NID that causes a dead-zone in which torques are not transmitted correctly.

On the other side, we can observe that the force is correctly estimated when using only the force and torque feedback, while some errors can appear when using only the tip position and orientation feedback. This can be explained as previously by the fact that the position of the tip provides little information on the force at the base once the needle is inserted in the tissues.

Overall it can be observed that using all the measures to perform the update provides a trade-off between the fitting of the different measures by the model. Whichever the kind of measure chosen to update the model, we can see in Fig. 3.20 that the position of the needle tip can be well estimated with an error under 2.5 mm while a 1.5 cm lateral motion is applied to the tissues. This can be sufficient in clinical practice to reach standard size tumors in the body while the patient is freely breathing. Results on the tissue motion estimation: Finally, let us compare the estimation of the position of the tissues to the measure provided by the odometry of the robot moving the phantom. The estimated and measured positions are shown in Fig. 3.23, as well as the absolute estimation error. It can be seen that the overall shape of the tissue motion is well estimated. However, some lag and drift in the estimation can be observed for all combinations of the measures.

In the case of the force measurements, the lag can be due to the play between the needle and the NID. Indeed, the tissues have to move from a certain amount and displace the needle before any force can actually be transmitted to the NID and be measured. This issue could be solved, along with the problem of torque measurement mentioned previously, by using a needle manipulator that provides a better fixing to the needle.

In the case of the tip position measurements, the drift can be due to modeling errors on the shape of the spline curve simulating the path cut in the tissues. Indeed, the extremity of this spline is progressively updated according to the position of the simulated needle tip during the insertion. However, modeling errors can lead to an incorrect shape of the spline, such that the estimation of the rigid translation of the tissues can not be done properly. A first solution could be to allow some deformations of the spline once it has been created, however this would introduce many additional parameters that need to be estimated. This can create observability issues and may require additional sensors, which is not desirable in practice. Another solution would be to directly use the position feedback provided on the needle tip to update the extremity of the spline. This solution will be explored in the following when using visual feedback on the entire needle shaft.

Conclusions:

We have seen that the update of the position of the tissues in our model could be done using a method based on the UKF with measures provided by force and torque feedback at the needle base and/or EM position feedback on the tip. Both modalities could provide good results by themselves such that it may not be required to use both at the same time. However they provide different kinds of information that may be used for different purposes, such as accurate targeting for the EM tracker and reduction of the forces applied on the tissues for the force sensor.

An additional advantage of using the force sensor is that it does not require a specific modification of the needle, contrary to the EM tracker that must be integrated into the needle before the insertion and removed before injecting something through the lumen of the needle.

Nevertheless, neither of them can provide a feedback on the position of a target in the tissues, such that an additional modality is required for needle insertion procedures.

On the contrary, medical imaging modalities can provide a feedback on a target as well as the position of the needle. Therefore in the next section we focus on the estimation of the tissue motions in our model by using the position feedback provided by an imaging modality.

Update from position feedback

In this section, we propose to test our tissue motion estimation method to update our interaction model using a 3D position feedback on the needle shaft. We focus here on the visual feedback provided by cameras to validate the algorithm. However it could be adapted to any other imaging modalities Figure 3.24: Experimental setup used to validate the performances of the tissue motion estimation algorithm when using the visual feedback provided by two cameras to detect the position of the needle body.

that can provide a measure of the needle localization, as will be done with 3D ultrasound (US) in the next section.

In the following we present the experiments that we performed to assess the quality of the model update obtained using the measures of the positions of several points along the needle. The performances are compared in terms of accuracy of the tip trajectory and estimated motions of the tissues.

Experimental conditions (setup in France): The setup used for these experiments is depicted in Fig. 3.24. The Angiotech biopsy needle is attached to the end effector of the Viper s650 and inserted in a gelatin phantom embedded in a transparent plastic container. The needle is inserted in the phantom without steering, i.e. the trajectory of the base of the needle simply describes a straight vertical line. Lateral motions are applied manually to the phantom during the insertion.

Visual feedback is obtained using the stereo cameras system. The whole needle shaft is tracked in real-time by the image processing algorithm described previously in section 3.4.1. The position of the phantom is measured from the tracking of two fiducial markers with four dots glued on each side of the container [START_REF] Horaud | An analytic solution for the perspective 4-point problem[END_REF] (see Fig. 3.24 and Fig. 3.27).

We use the two-body model presented in section 2.4.1 with polynomial needle segments of order r = 3. We fix the length of the needle segments to 1 cm, resulting in a total of n = 13 segments and the last segment measuring 0.6 mm. The stiffness per unit length of the model is set to 3200 N.m -2 and the length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm.

Model update:

We propose to compare five different methods to represent the needle and to update the spline curve representing the path cut in the tissues in our model, as described in the following:

• Method 1: the needle is modeled as a straight rigid needle.

• Method 2: the needle is modeled using the two-body flexible needle model. The extremity of the tissue spline is updated using the cutting edge of the modeled bevel, as was described in the definition of the model in section 2.4.2.

• Method 3: similar to method 2, except that the extremity of the tissue spline is updated using the visual feedback instead of the model of the bevel. The segment is added to link the last added segment to the current position of the real needle tip measured from the camera visual feedback. However the position of the whole tissue spline is not modified during the insertion.

• Method 4: similar to method 2 with the addition of the proposed update algorithm based on unscented Kalman filter (UKF) to estimate the position of the tissue spline from the measured position of the needle.

• Method 5: similar to method 3 with the addition of the proposed update algorithm based on UKF to estimate the position of the tissue spline from the measured position of the needle.

For each method, the position of the simulated needle base is updated during the insertion using the odometry of the robot. For methods 4 and 5, we use the positions of several points along the needle as input for the update algorithm (as described by (3.86) and (3.87) in section 3.5.2.2). The points are extracted 5 mm from each other along the 3D polynomial curve obtained from the needle tracking using the cameras.

The measurement noise covariance matrix R in the UKF is set as a diagonal matrix with diagonal elements equal to (0.25) 2 mm 2 , corresponding to the accuracy of the stereo system used to get the needle points. The process noise covariance matrix Q is set as a diagonal matrix with diagonal elements equal to (0.1) 2 mm 2 .

Experimental scenario: Five insertions at different locations in the phantom are performed. The needle is first inserted 1 cm in the phantom to be able to initialize the needle tracking algorithm described in section 3.4.1. The insertion is then started, such that the needle base is only translated along the needle axis. The phantom is moved manually along different trajectories for each insertion, such that the motions have an amplitude up to 1 cm in the x and y directions of the world frame {F w }as depicted in Fig. 3.24.

Results:

We present now the results obtained during the experiments. We first consider the accuracy of the simulated tip trajectories and evaluate the effect of the update rate on this accuracy. The quality of the estimation of the motions of the tissues is then assessed and we discuss some limitations of the modeling.

Comparison of tip trajectories:

We first compare the tip trajectories obtained with the different model update methods. The average absolute position error between the measured and simulated needle tips calculated over time and across the different experiments is summarized in Fig. 3.25 and Table 3.1. An example of measured and simulated tip positions obtained during one experiment is shown in Fig. 3.26. Figure 3.27 shows the corresponding pictures of the needle acquired with the cameras at different steps of the insertion. The tissue spline corresponding to each model is overlaid on the images at each step.

It is clearly visible from the simulated tip trajectories that updating the model while taking into account the motions of the tissues is crucial to ensure that the model remains a good representation of the real needle. It can also be observed from the mean absolute error over all the experiments in Fig. 3.25, that the more the model is updated, the better it fits the reality. However we can see that method 3 yields poor results since only the extremity of the tissue spline is updated by adding new segments that fit the measured positions of the tip. Since the lateral position of the spline is not updated to account for tissue motions, the resulting shape of the spline does not correspond to the reality, as can be seen in Fig. 3.27 (blue curve). On the contrary, modifying the whole position of the spline in addition to the update of its extremity allows taking into account the lateral motions of the tissues, as is done with methods 4 and 5. These results illustrate that a feedback on the needle is a necessity during insertion procedures. Indeed, a pre-operative planning would not be sufficient to predict the real trajectory of the flexible needle, as is illustrated by the trajectories of the non-updated models (method 1 and 2). Therefore, the association of the needle model and update algorithm that we propose proves to be a good method to accurately represent the current state of the insertion process.

However, 3D medical imaging modalities typically have an acquisition time that is longer than the framerate of the cameras used in these experiments, such that the update can only be performed at a lower rate. Hence, we propose to compare the results obtained using two different update rates for the update methods that use the visual feedback (methods 3, 4 and 5).

Effect of update rate: In order to simulate a slower imaging modality, like the 3D US that we will use in the following, we set the update rate to 1 Hz, meaning that the update of the tissue spline is performed only once every second. However, the update of the position of the needle base from the robot odometry is still performed at the fast rate available with the robot.

The resulting error between the measured and simulated needle tip trajectories during the example experiment can be seen in Fig. 3.28. The average tip position errors calculated over time and across the different experiments are also summarized in Fig. 3.25 and Table 3.1. As expected, a higher update rate (30 Hz) provides better results than a lower update rate (1Hz), since more measures can be taken into account to estimate the position of the tissues. However, regularly updating the model even at a low rate still allows a good reduction of the modeling errors that occurred between two acquisitions, such that we can expect good results from the algorithm with 3D US.

Estimation of tissue motions: Now that we have illustrated the importance of updating the interaction model to ensure a good modeling of the needle during the insertion procedure, we propose to evaluate the performances of the update algorithm to see if it can actually be used to estimate the real motions of the tissues.

The position of the phantom is obtained by the tracking of the fiducial markers placed on the container, as can be seen in Fig. 3.27. The measured positions of the tissues during the previous insertion example are presented in Fig. 3.29 along with the estimation provided by the method 4. The results using the slower update rate are also shown.

Overall the update method allows the tracking of the motions of the tissues and similar results are observed for both high and low update rates. This can also be observed in Fig. 3.27, on which it is visible that the updated tissue splines from method 4 and 5 follows the motion of the tissues around the needle. We further discuss the quality of the estimation in the following.

Limitations of the model: Some tracking errors can still be observed on the position of the tissues when updating the model. They can be due to the accumulation of errors in the shape of the tissue spline, as was also discussed in previous section 3.6.1 when using force and position feedback. The same solution that was proposed could also be used here, consisting in updating the whole shape of the tissue spline instead of only its global translation. However, it is very likely to see observability issues appearing, due to the fact that different shapes of the tissue spline can lead to similar needle shapes. Additional phenomena can explain the tracking errors, such as the nonlinear properties of the tissues on which we briefly focus in the following. During some other of our experiments, some large lateral motions were applied to the base of the needle, such that the needle was cutting laterally in the gelatin and a tearing appeared at the surface. In this case the needle is moving inside the tissues without external motion of the tissues. The results of the tissue motion estimation using the update method 4 in this case are shown in Fig. 3.30. The tearing of the gelatin occurred at the beginning of the insertion, from t = 2.5s to t = 4.5s. We can see that the model is automatically updated according to the measures of the needle position, so that a drift appears in the estimated position of the tissues. Once the needle has stopped cutting laterally in the gelatin (at t = 4.5), the needle is embedded anew in the tissues. This is equivalent to changing the rest position of the cut path associated to the real needle and this is what is actually represented by the tissue spline of the updated model. Hence the following motions of the tissues are well estimated, although the drift remains. Figure 3.30: Example of tissue motions estimated using the update method 4 with the position feedback obtained from cameras. At the beginning of the insertion (blue zone from t = 2.5s to t = 4.5s), the needle base is moved laterally such that a tearing occurs at the surface of the gelatin. This creates an offset in the estimation of the motions of the tissues.

Even if the tearing of the tissues is less likely to appears in real biological tissues, this example shows that our model and update method can lead to a wrong estimation of the real position of the tissues due to unmodeled phenomena. However, it can also be noted that if the simulated position of the tissues was updated according to an external position feedback on the real tissues, for example by tracking a marker on the surface of the tissues, the resulting state of the model would poorly fit the position of the real needle. On the contrary, our update algorithm using the position of the needle allows the model to fit the measures provided on the needle and to remain consistent with the way it locally represents the tissues. This can be seen as an advantage of the method since the goal of our model is to give a good estimation of the local behavior of the needle without modeling all the surrounding tissues.

Conclusions: From the results of these experiments we can conclude that the method that we proposed to update the state of our model based on the UKF allows taking into account the effect of tissue motions on the shape of the needle.

TISSUE UPDATE VALIDATION

We have also seen that the non-linear phenomena occurring in the tissues, such as a lateral cutting by the needle, can have a great impact on the quality of the estimation of the real position of the tissues. In practice, real tissues are less prone to tearing than the gelatin used in the experiments and the needle will also be steered to avoid such tearing, however the hyper-elastic properties of real biological tissues may induce a similar drift in the estimation. Therefore, the update algorithm will not be used in the followings as a way to measure the exact position of the tissues, but only as a way to keep the model in a good state to represent the local behavior of the needle.

We could also see that the method provides a good update even when considering the low acquisition rate that is available with a slower, but still real-time, imaging modality, such as 3D US. Hence, in the next section we use the update method as a way to increase the modeling accuracy of the needle insertion, such that it can be used as a prediction tool to improve the tracking of a needle in 3D US volumes.

Needle tracking in 3D US with moving soft tissues

In this section we propose to combine the model update method based on unscented Kalman filter (UKF) that was designed in section 3.5.2 with the needle tracking algorithm in 3D ultrasound (US) proposed in section 3.4.2. This combination is used to provide a robust tracking of a needle in a sequence of 3D US volumes during an insertion in moving tissues.

In the previous section we used the visual feedback provided by cameras to track the needle and update the needle model to take into account the lateral motions of the tissues. However, the position of the tracking system was registered beforehand in the frame of the robotic needle manipulator by observing the needle in the acquired images, as described in section 3.4.1.

In the case of a 3D US probe, a similar registration of the pose of the probe would require many insertions of the needle in the tissues to be able to observe its position in the US volume. This is not possible in a clinical context, in which multiple insertions should be avoided and where the registration process should be simple and not time consuming. Therefore, we propose to use a fast registration method performed directly at the beginning of the insertion procedure.

In the following we present the results of the experiments that we performed to assess the performances of the tracking method combining our contributions.

Experimental conditions (setup in France):

The Angiotech biopsy needle is used and attached to the end effector of the Viper s850. The insertion is done vertically in a gelatin phantom embedded in a transparent plastic container. The container is fixed to the end effector of the Viper s650, which is used to apply a known motion to the phantom.

We use the 3D US probe and US station from BK Ultrasound to grab online 3D US volumes. The US probe is fixed to the same table on which the phantom is placed, such that it is perpendicular to the needle insertion direction and remains in contact with the phantom. The acquisition parameters of the US probe are set to acquire 31 frames during a sweeping motion with an angle of 1.46 • between successive frames. The acquisition depth is set to 10 cm, resulting in the acquisition of one volume every 630 ms and a maximal resolution of 0.3 mm × 1 mm × 2 mm at the level of the needle, which is approximately 5 cm away from the probe. The spacial resolution of the post-scan volume is set to 0.3 mm in all directions and linear interpolation is used for the reconstruction.

Tracking method: We use the tracking algorithm proposed in section 3.4.2 that exploits US artifacts to track the needle in the acquired sequence of US volumes. For each new volume acquisition, the tracking is initialized using three different methods described in the following:

• Method 1: the tracking is initialized from the position of the needle tracked in the previous volume. No model of the needle is used in this case.

• Method 2: the tracking is initialized using the projection of the needle model in the 3D US volume. We use the two-body model presented in section 2.4.2 with polynomial needle segments of order r = 3. We fix the length of the needle segments to 1 cm, resulting in a total of n = 13 segments and the last segment measuring 0.6 mm. The stiffness per unit length of the model is set to 3200 N.m -2 and the length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm.

The model is updated between two volume acquisitions using only the odometry of the Viper s850 to defined the position of the simulated needle base.

• Method 3: the same process as method 2 is used, except that the model is updated with the method presented in section 3.5.2 to take into account the motions of the tissues. Similarly to the experiments performed in previous section with camera feedback, we use the positions of several points separated by 5 mm from each other on the needle body as inputs for the UKF.

The measurement noise covariance matrix R in the UKF is set with diagonal elements equal to (2) 2 mm 2 and the process noise covariance matrix Q with diagonal elements equal to (3) 2 mm 2 .

TISSUE UPDATE VALIDATION

Note that the needle model is defined in the frame of the robot, since the position of the simulated base is set according to the robot odometry. A registration between the US volume and the robot is thus necessary for the update method in order to convert the position of the needle body tracked in the volume to the robot frame.

Registration:

We describe here the registration method that we use to find the correspondence between a voxel in a 3D US volume and its real location in the needle manipulator frame. The US volumes are first scaled to real Cartesian space by using the size of a voxel, which is known from the characteristics of the probe and the process used to convert pre-scan data into post-scan data (as explained in section 3.2.2.2).

In order to be in accordance with our objective of a reduced registration time and complexity, we use a fast registration method that can be used directly at the beginning the insertion procedure. After an initial insertion step, the part of the needle that is visible in the acquired US volume is manually segmented, giving both tip position and orientation. The pose of the volume is then computed by matching the measured tip position and orientation to the position and orientation obtained from the needle manipulator odometry and the needle model. The manual needle segmentation is also used for the initialization of the needle tracking algorithm.

Note, however, that this method provides a registration accuracy that depends on the quality of the manual needle segmentation.

Experimental scenario: We perform 10 straight insertions of 10 cm at different locations in the gelatin phantom with an insertion speed of 5 mm.s -1 . The needle is first inserted 1 cm in the phantom and manually segmented in the US volume to initialize the different tracking algorithms and register the probe pose. The insertion is then started at the same time as the motion applied to the phantom.

For each experiment, a similar 1D lateral motion is applied to the container such that the phantom always stays in contact with both the table and the US probe. The motion follows a profile m(t) similar to a breathing motion [HMB + 10], expressed as

m(t) = b cos 4 ( π T t - π 2 ), (3.97)
where t is the time, b is the magnitude of the motion, set to 1 cm, and T is the period of the motion, set to 5 s. The insertion is performed for a duration of 18 s, roughly corresponding to 4 periods of the motion and the acquisition of 29 volumes.

Results on tip tracking: An example of the positions of the tip tracked during one experiment using the different methods is shown in The insertion is performed along the y axis of the probe while the lateral motion of the tissues is applied along the z axis. One tracking method is initialized without model of the needle (blue), one is initialized from a model of the needle that does not take into account the motions of the tissues (green) and one is initialized from a model updated using the tissue motion estimation algorithm presented in section 3.5.2 (red).

along with the ground-truth acquired by manual segmentation of the needle tip in the volumes. Figure 3.32 shows the result of the needle tracking algorithms in two orthogonal cross sections of the volume near the end of the insertion. We can observe that tracking the needle without any a priori information on the needle motion occurring between two volume acquisitions (method 1) leads to a failure of the tracking as soon as the beginning of the insertion. The tracking get stuck on the artifact appearing at the surface of the gelatin, due to the fast lateral velocity of the tissues as well as the low visibility of the needle at the beginning of the insertion. We can see that the tracking is able to follow the motion of the tissues (z axis in Fig. 3.31) since the artifact moves with the phantom. However the length of the inserted part of the needle is not provided to the algorithm, so that it stays at the surface without taking into account the insertion motion, as can be seen in Fig. 3.32.

On the contrary, using the needle model to initialize the tracking allows taking into account the length of the needle that is currently inserted, such that both methods 2 and 3 provide a good estimation of the tip position along the y axis. However, when the model is only updated at its base (method 2), the tracking mostly fails due to the wrong lateral location of the initialization that does not take into account the motions of the tissues. We can see that the tracking is rather inconsistent in this case. Sometimes the tracking recovers the correct location of the needle when it is near the model, as can be seen in Fig. 3.31 from volume 13 to 16 (green curve); and when the tissues move far from their initial position, the tracking is initialized near other structures or artifacts, such that it fails to find the needle, as is the case in Fig. 3.32.

On the other hand, updating the model according to the tracked position of the needle (method 3) allows taking into account the motions of the tissues. This way, the prediction of the needle localization in the following volume is of good quality and the tracking algorithm can accurately find the position of the needle. Overall the combination of the tracking algorithm with the updated model allows a good tracking of the needle tip with a mean accuracy of 3.1 ± 2.5 mm over the volume sequences of all the insertions, which may be sufficient for commonly performed needle insertions.

Results on tissue motion estimation: As a final consideration, let us have a look at the estimated position of the tissues in the updated model that is provided in Fig. 3.33. We can see that the overall motions of the tissues are well estimated by the algorithm. However, we can observe a delay between the estimation and the measures. Although a part of this delay may be introduced by the filtering effect of the UKF, it is most probably due to the delay introduced by the acquisition time required to obtain the final 3D US volume. This issue could be solved by taking into account the known time required by the system to reconstruct the volume from the data acquired by the US transducer.

We can also observe a slight drift of the estimation during the insertion. This can be due to the accumulation of modeling errors that can arise because of some local tearing of the gelatin when the phantom moves far from its initial position. It can also come from the fast registration method that we used in these experiments, which can introduce a difference between the real position of the needle and the measured position reconstructed from the tracking in the volume.

These observations confirm the fact that has already been discussed in previous section, namely that updating the position of the tissues in the model should only be used as a way to get a good representation of the needle by the model and not an accurate measure of the tissue position.

Conclusions:

We provided a method for improving the robustness of the tracking of a flexible needle in 3D US volumes when the tissues are subject to lateral motions. Using a mechanics-based model allows a prediction of the motions of the needle tip and shaft due to the motions of the needle manipulator between two volume acquisitions. The prediction can then be used to provide a good initialization of an iterative needle tracking algorithm. Finally, updating the model thanks to the result of the tracking allows taking into account the motions of the tissues and improves the modeling accuracy and the subsequent prediction.

The quality of the prediction of the needle location could even be further improved by using a fast information feedback to update the modeled position of the tissues between consecutive volume acquisitions. This could be done using a force sensor or an electromagnetic tracker, as we have demonstrated in section 3.6.1. However, an imaging modality remains a necessity to achieve the steering of the needle toward a target.

Conclusion

In this chapter, we started by a brief comparison of the imaging modalities traditionally used to perform needle insertions. From this we chose to focus on the ultrasound (US) modality and we presented the general principles of US imaging as well as the way to reconstruct 2D images or 3D volumes that can then be exploited. We also covered the case of several artifacts that are specific to the presence of a needle in the field of view of the US probe.

A review of current detection and tracking methods used to localize a needle from 2D or 3D US feedback was then provided. We proposed a first contribution in this field consisting in an iterative algorithm that exploits the artifacts observed around a needle to accurately find the position of its whole body in a 3D US volume. The performances of the algorithm were illustrated through an experimental validation and a comparison to another state-of-the-art algorithm.

Then we considered the case of a change of position of the tissues due to motions of the patient. We presented the concepts of Bayesian filtering and proposed an algorithm based on an unscented Kalman filter to update the state of the interaction model that we developed in chapter 2 using the different measures available on the needle. We have shown through various experimental scenarios that the update method could be used with several kinds of information feedback on the needle, such as force feedback, electromagnetic position feedback or visual position feedback, in order to take into account the lateral motions of the tissues. We then proposed to fuse our two contributions into one global method to mutually improve both tracking performances in 3D US and insertion modeling accuracy.

Good localization of the needle and accurate modeling of the insertion are two important keys to provide an image-guided robotic assistance during an insertion procedure. Now that we have addressed these two points and have proposed a contribution for both of them, we will focus in chapter 4 on the design of a control framework for robotic needle insertion under visual guidance.

Chapter 4

Needle steering

In this chapter we address the issue of steering a flexible needle inserted in soft tissues. The goal of a needle insertion procedure is to accurately reach a targeted region embedded in the body with the tip of the needle. Achieving this goal is not always easy for clinicians due to the complex behavior exhibited by a thin flexible needle interacting with soft tissues.

Robot assisted needle insertion can then be of great help to improve the accuracy of the operation and to reduce the necessity of repeated insertions. In chapter 2 we presented different ways of modeling the insertion of a flexible needle in soft tissues. In particular we have seen that kinematic and mechanics-based models offer a reasonable computational complexity that makes them suitable for real-time processing and control of a robotic system.

In the following, we first provide in section 4.1 a review of current techniques used to steer different kinds of needles using a robotic system. Then we present different methods in section 4.2 used to define the trajectory that the needle tip must follow to reach a target and avoid obstacles. In section 4.3 we propose a new contribution consisting in a generic needle steering framework for closed-loop control of a robotic manipulator holding a flexible needle. This framework is based on the task function framework and can be adapted to steer different kinds of needles. It is formulated such that different kinds of sensing modalities can be used to provide a feedback on the needle and the target. We finally describe different experimental scenarios in section 4.4 that we use to assess the performances of our steering framework.

Parts of the work presented in this chapter on the steering framework were published in two articles presented in international conferences [CKB16a] [CKB16b].

Steering strategies

In this section we present a review of current techniques used to control the trajectory of the tip of a needle inserted in soft tissues. The techniques used to reach a target in soft tissues while avoiding other sensitive regions can be gathered into three main families.

• Tip-based steering methods use a needle with an asymmetric design of the tip to create a deflection of the tip trajectory when the needle is inserted into the tissues without any other lateral motion of its base.

• Base manipulation methods on the contrary use lateral translation and rotation motions of the needle base during the insertion to modify the trajectory of the needle tip.

• Lastly, tissue manipulation is a special case in the sense that no needle steering is actually performed. Instead it uses deformations of the surrounding tissues to modify the position of the target and obstacles.

We present each steering family in further detail in the following.

Tip-based needle steering

As described in the section on kinematic modeling 2.1, it can be observed that the presence of an asymmetry of the needle tip geometry, such as a bevel, leads to a deviation of the needle trajectory from a straight path, as illustrated in Fig. 4.1a. Considering this effect as a drawback, clinicians usually rotate the needle around its axis during the insertion to cancel the effect of the normal component of the reaction force created at the needle tip. This allows the trajectory of the needle tip to follow a straight line. However many research works have been conducted over the last two decades to use this effect as an advantage to steer the needle tip, leading to the creation of the tip-based steering strategies [APM07] [START_REF] Van De Berg | Design choices in needle steering-a review[END_REF]. Tip-based needle steering consists in controlling the orientation of the lateral component of the reaction force at the tip to face a desired direction. The behavior of the needle tip can usually be accurately modeled using kinematic models [WIKC + 06]. Needles used for tip-based control typically have a small diameter and are made of super-elastic alloys, such as Nitinol, to decrease the needle rigidity and to increase the influence of the tip force on the needle trajectory. This allows getting closer to the assumption that the needle is very flexible with respect to the surrounding tissues, which is required for the validity of kinematic models (see section 2.1).

The control of the insertion of such needles is often limited to the insertion of the needle along its base axis and the orientation of the needle around this axis. Different control strategies have been developed to steer the needle tip using only these two degrees of freedom (DOF). A constant ratio between 4.1. STEERING STRATEGIES the rotation and insertion velocities of the needle can be used to obtain a helical trajectory [HAC + 09]. A low ratio leads to a circular trajectory with curvature corresponding to the natural curvature of the needle insertion. A high ratio leads to an almost straight trajectory.

Duty-cycling:

The duty cycling control strategy, first tested in [START_REF] Engh | Toward effective needle steering in brain tissue[END_REF] and later formalized in [START_REF] Minhas | Modeling of needle steering via duty-cycled spinning[END_REF], consists in using alternatively only the two extreme cases of the helical trajectories: pure insertion of the needle (maximal curvature of the trajectory) and insertion with fast rotation (straight trajectory). The resulting trajectory of the needle tip can be approximated by an arc of a circle with an effective curvature K ef f that can be tuned between 0 and the maximal curvature K nat . It has been shown that the relation between K ef f and the duty cycle ratio DC between the length of the phases could be approximated by a linear function [START_REF] Minhas | Modeling of needle steering via duty-cycled spinning[END_REF]:

DC = L rot L rot + L ins , (4.1) K ef f = (1 -DC)K nat , (4.2)
where L ins and L rot are the insertion lengths corresponding respectively to the pure insertion phase and the insertion phase with fast rotation. Similarly, in the case of a constant insertion velocity, the duty-cycle DC can be computed from the duration of each phase instead of their insertion length. This method has first been used only in 2D, using an integer number of full 2π rotations during the rotation phase [START_REF] Minhas | Modeling of needle steering via duty-cycled spinning[END_REF]. It was later extended to 3D by adding an additional angle of rotation before the insertion phase to orient the curve toward the desired direction [START_REF] Wood | Algorithm for three-dimensional control of needle steering via duty-cycled rotation[END_REF]. A 3D kinematic formulation was also proposed by Krupa [Kru14] and Patil et al. [START_REF] Patil | Needle steering in 3-d via rapid replanning[END_REF]. Duty-cycling control has also been extensively used in its 2D or 3D versions over the past decade, associated with various needle insertion systems, needle tracking algorithms and methods to define the trajectory of the tip (see for example [vdBPA

+ 11] [BAP + 11] [PBWA14] [CKN15] [MPT16]
). Trajectory planning will be covered in next section 4.2.

Duty-cycling control presents some drawbacks that have to be addressed. First the natural curvature K nat must be known to compute the duty-cycle DC. This parameter is difficult to determine in practice and may even vary with the insertion depth, such that an online estimation can be required [START_REF] Moreira | Needle steering in biological tissue using ultrasound-based online curvature estimation[END_REF]. It may also not be possible to continuously rotate the needle along its axis. This is for example the case when using cabled sensors attached to the needle, such as electromagnetic trackers or optic fibers embedded in the needle. The duty cycling control has to be adapted in this case to alternate the direction of the rotation around the needle shaft [START_REF] Majewicz | Design and evaluation of duty-cycling steering algorithms for robotically-driven steerable needles[END_REF].

The effect of the bevel angle on the needle insertion has been studied in artificial [START_REF] Webster | Design considerations for robotic needle steering[END_REF], ex-vivo [START_REF] Majewicz | Evaluation of robotic needle steering in ex vivo tissue[END_REF] or in-vivo [MMVV + 12] tissues. It has been observed that it has a direct effect on the amount of deflection of the needle tip from a straight path. However the curvature of the tip trajectory is very low in biological tissues, which can limit the interest of using the duty-cycling control in clinical practice. The natural curvature of the needle can be increased by using a needle with a prebent [AGL + 16] or precurved tip [VDBDJVG + 17]. However this is not suitable for duty-cycling control since it also increases the damage done to the tissues during the rotation of the needle.

Special design: Particular mechanical designs of the needle have been proposed to control the force created at the needle tip. Swaney et al. [START_REF] Swaney | Webster. A flexure-based steerable needle: High curvature with reduced tissue damage[END_REF] designed a specific flexure based needle tip to offer the high curvature of a prebent-tip needle during insertion while keeping the reduced tissue damage of a beveled-tip needle during rotations.

Active tips were also designed to allow a modification of the lateral force intensity and orientation without using rotation of the needle around its axis. Burrows et al. [START_REF] Burrows | Smooth online path planning for needle steering with non-linear constraints[END_REF] use a needle made of multiple segments that can slide along each other, thus modifying the shape of the tip of the needle. Shahriari et al. [SRvdB + 16] use a tendon-actuated needle tip with 2 DOF, which acts as a pre-bent tip with a variable tip angle and orientation.

The main drawbacks of tip-based steering are that the tip trajectory can only be modified by inserting the needle and that the amplitude of the obtained lateral motions is relatively small in real clinical conditions. Although special designs have been proposed to offer improved steering capabilities, these needles are still unsuitable for a fast and low cost integration into clinical practice. However, other steering methods can be used to steer traditional needles, as we will see in the following.

Needle steering using base manipulation

Base manipulation consists in controlling the needle tip trajectory using an adequate control of the 6 degrees of freedom (DOF) of the needle base. In the case of a symmetric tip needle, changing the trajectory of the needle tip from a straight path requires bending the needle and pushing laterally on the tissues, as illustrated in Fig. 4.1b. This is the natural way clinicians use to steer a needle when holding it by its base.

Pioneer work on robotic control of a needle attached by its base to a robotic manipulator was performed by DiMaio et al. [START_REF] Dimaio | Needle steering and motion planning in soft tissues[END_REF]. The flexibility of the needle and its interaction with soft tissues was modeled using 2D finite element modeling (FEM) and was used to predict the motion of the needle tip resulting from a given needle base motion. The model was used to compute the trajectory of the needle base that would result in the desired tip trajectory. Due to the computational complexity of the FEM, only preplanning of the needle trajectory was performed and the actual insertion was performed in open-loop control. Closed-loop needle base manipulation was performed under fluoroscopic guidance by Glozman and Shoham [START_REF] Glozman | Image-guided robotic flexible needle steering[END_REF] and later under ultrasound guidance by Neubach and Shoham [START_REF] Neubach | Ultrasound-guided robot for flexible needle steering[END_REF]. The 2D virtual springs model was used in both cases to perform a pre-planning of the needle trajectory that also minimizes the lateral efforts exerted on the tissues. Additionally, this mechanics-based model enabled real-time performance and was used in the closed-loop control scheme to ensure that the real needle tip follows the planned trajectory.

Despite being among the first work on robotic needle steering, base manipulation has been the subject of little research these past years compared to the amount of work on tip steerable needles. This can mainly be explained by the fact that bending the needle and pushing on the tissues to control the lateral motion of the tip can potentially induce more tissue damage than only inserting the needle. The efforts required to induce significant lateral tip motion also rapidly increase as the needle is inserted deeper into the tissues. This can limit the use of base manipulation to superficial targets.

However it can also be noted that the 2 DOF used in tip-based control (translation and rotation along and around the needle axis) can also be controlled using base manipulation. Therefore it is also possible to use a base manipulation framework to perform tip-based steering of a needle with an asymmetric tip, as illustrated in Fig. 4.1c. Using only tip-based control, the needle base can only translate in one insertion direction and it is not possible to compensate for any lateral motions of the tissues that may arise from patient motion. On the contrary, using all 6 DOF of the needle base offers the advantage of keeping additional DOF if necessary. Therefore, due to its ability to handle both symmetric and asymmetric tips, base manipulation in the general sense is the steering method that we choose to explore in the following.

Tissue manipulation

Tissue manipulation consists in applying deformations on the internal parts of the tissues by moving one [THA + 09] or multiple points [START_REF] Mallapragada | Robotassisted real-time tumor manipulation for breast biopsy[END_REF][PVdBA11] of the surface of the tissues. This kind of control requires an accurate finite element modeling (FEM) model of the tissues, which is difficult to obtain in practice due to parameter estimation. The computational load of FEM is also an obstacle for real-time use, limiting it to pre-planning of the insertion procedure, which further enhances the need for an accurate modeling. This technique has only been used so far to align the target with a large rigid needle and no work has been conducted to explore the modification of the trajectory of a flexible needle. In addition, it can be observed that the motion of the tissue surface has a little influence on the motion of deep anatomical structures: tissue manipulation can then only be used to move superficial targets. Shallow targets are not the only kind of targets that we want to cover in our work, therefore we do not consider tissue manipulation in the following.

Needle tip trajectory

In section 4.1, we presented different methods to control the motion of the tip of a needle being inserted in soft tissues using a robotic manipulator. Once a type of needle and an associated control scheme has been chosen to control the needle tip, a strategy needs to be chosen to define the motion to apply to the needle tip. Two approaches are generally used, which are path planning and reactive control.

The path planning approach uses some predictions of the behavior of the system and tries to find the best sequence of motions that needs to be applied to fulfill the general objective. On the contrary, the reactive control approach only relies on the current state of the system and intra-operative measures to compute the next motion to apply.

Path planning

Path planning is used to define the entire trajectory that needs to be followed by the needle tip to reach the target. This approach requires a model of the needle insertion process to predict the effect of the control inputs on the tip trajectory. It is mostly used in tip-based steering, for which the unicycle model (see section 2.1) can be used because of its simplicity and computational efficiency.

Planning the natural trajectory: Duindam et al. [DXA + 10] planned the trajectory of the needle while considering a stop-and-turn strategy, thus alternating between rotation-only phases and insertion-only phases. Three insertion steps were considered, leading to a tip trajectory following three successive arcs with constant curvature. The best duration of each phase, i.e. the length of each arc, was computed such that the generated trajectory reached the target.

Hauser et al. [HAC + 09] exploited the helical shape of the paths obtained when applying constant insertion and rotation velocities to the needle. The best velocities were computed by selecting the helical trajectory that allowed the final tip position to be the closest to the target. A model predictive control scheme was used, in which the best selected velocities are applied for a short amount of time and the procedure is repeated until the target has been reached.

Rapidly-exploring random tree (RRT): Among the many existing path planning algorithms, the RRT algorithm [START_REF] Lavalle | Randomized kinodynamic planning[END_REF] has been widely used in needle steering applications. This probabilistic algorithm consists in randomly choosing multiple possible control inputs and generating the corresponding output trajectories. The best trajectory is then chosen and the corresponding control inputs are applied to the real system. The RRT can be used in many ways, depending on the underlying model chosen to relate the control inputs to the output tip trajectory.

The first use of RRT for 3D flexible needle insertion planning was done by Xu et al. [START_REF] Xu | Motion planning for steerable needles in 3d environments with obstacles using rapidly-exploring random trees and backchaining[END_REF]. The kinematic model of the needle with constant curvature was used to predict the motion of the needle tip for given insertion and rotation velocities. Due to the constant curvature constraint the control inputs were limited to a stop-and-turn strategy. However, a lot of trajectories had to be generated before finding a good one: the algorithm was then slow and could only be used for pre-operative planning of the insertion.

The introduction of the duty-cycling control allowed dropping the constant curvature assumption and consider the possibility of controlling the effective curvature of the tip trajectory. This simplified the planning and online intra-operative replanning could be achieved in 2D [BAP + 11] and 3D [PA10] [START_REF] Bernardes | 3d robust online motion planning for steerable needles in dynamic workspaces using duty-cycled rotation[END_REF]. The RRT was also used with 2D finite element model-123 CHAPTER 4. NEEDLE STEERING ing instead of kinematic modeling to provide a more accurate offline preoperative planning that takes into account the tissue deformations due to the needle insertion [START_REF] Patil | Motion planning under uncertainty in highly deformable environments[END_REF].

Planning under uncertainties: Since planning methods always rely on the predictions given by a model of the needle, inaccuracy of the model can diminish the performances of the planning if they are not taken into account. Stochastic planning methods have been proposed to consider uncertainties on the motion of the tip.

Park et al.

[PKZ + 05] used a path-of-probability approach, where a stochastic version of the kinematic model is used to compute the probability density function of the final position of the needle tip. This is then used to generate an set of tip trajectories that can reach the target.

Alterowitz et al. [START_REF] Alterovitz | The stochastic motion roadmap: A sampling framework for planning with markov motion uncertainty[END_REF] used a stochastic motion roadmap to model the probability to obtain a given 2D pose of the needle tip starting from another tip pose. The optimal sequence of control inputs was then computed from the map to minimize the probability to hit an obstacle and to maximize the probability to reach the target. Fuzzy logic was also proposed as a way to cope with incertainty in the control inputs [START_REF] Lee | A probability-based path planning method using fuzzy logic[END_REF].

Even when modeling the uncertainty, unpredicted tissue inhomogeneities or tissue motions can greatly modify the trajectory of the tip or the geometry of the environment. Using pre-operative planning usually requires the use of reactive control during the real procedure to ensure that the planned trajectory can be accurately followed. Planning can be used to not only plan a feasible trajectory but also to design an optimal controller that can take into account the uncertainties on the model and the intra-operative measures during the procedure. In [vdBPA + 11] a linear-quadratic Gaussian (LQG) controller was designed to robustly follow the trajectory that was pre-planned using RRT. The controller could take into account the current state uncertainty to minimize the probability to hit an obstacle. Sun and Altrerovitz [START_REF] Sun | Motion planning under uncertainty for medical needle steering using optimization in belief space[END_REF] proposed to take into account the sensor placement directly during the design of the planning and LQG controller, in order to minimize the uncertainty on the tip location along the planned trajectory. This way, obstacles could be avoided without having to pass far away from them to ensure avoidance.

Online re-planning: Online re-planning of the trajectory can also be used instead of considering uncertainties in a model. By regularly computing a new trajectory that takes into account the current state of the insertion, the control can directly compensate for modeling uncertainties [BAPB13] [PBWA14]. This offers the good prediction capabilities of the planning approach while maintaining a good reactivity to environment changes, which is one of the motivations behind the research on fast planning algorithms that could work in real-time.

However, online re-planning is only possible when using simplified models like kinematic models. In the case of base manipulation control, the whole shape of the needle needs to be modeled, limiting the use of such model to pre-operative planning. Reactive control can then be used during the insertion to adapt to the changes in the environment.

Reactive control

Reactive control consists in using only a feedback on the current state of the system to compute the control inputs to apply. This kind of control usually uses inverse kinematics to compute the control inputs to obtain a desired output motion. If the approach does not rely on an accurate modeling of the system, it uses closed-loop control to compensate for modeling errors.

Reactive control with tip-based control: In the case of beveled-tip needles, sliding mode control can be used to control the bevel orientation during the insertion such that the bevel cutting edge is always directed toward the target. The advantage of this method is that it does not rely on the parameters of an interaction model with the tissues. Rucker et al. [RDG + 13] demonstrated that an arbitrary accuracy could be reached with this method by choosing an appropriate ratio between insertion and rotation velocities. Sliding mode control have proven its efficiency with many feedback modalities, such as electromagnetic (EM) tracker [RDG + 13], fiber Bragg grating (FBG) sensors [START_REF] Abayazid | 3d flexible needle steering in soft-tissue phantoms using fiber bragg grating sensors[END_REF], ultrasound (US) imaging [START_REF] Abayazid | Integrating deflection models and image feedback for realtime flexible needle steering[END_REF][FRS + 16] or computerized tomography (CT)-scan fused with EM tracking [SHvK + 17]. For this reason, we will include it in our control framework in the following section 4.3.

Reactive control can also be used to intra-operatively compensate for deviations from a trajectory that has been planned pre-operatively by another planning algorithm. Sliding mode control can for example be adapted to follow keypoints along the planned trajectory instead of directly pointing toward the target [AVP + 14]. The linear-quadratic Gaussian (LQG) control framework can also be used to take into account modeling errors and measure noise during the insertion [START_REF] Kallem | Image guidance of flexible tip-steerable needles[END_REF].

Since reactive control is expected to work in real-time, kinematic models are most often used. However such models are only applicable for needle with asymmetric tips whereas base manipulation must be used in the case of a symmetric tip needle.

Reactive control with base manipulation:

The first robotic needle insertion procedure using base manipulation [START_REF] Dimaio | Needle steering and motion planning in soft tissues[END_REF] proposed to use vector fields to define the trajectory that needed to be followed by the needle tip. The needle and tissues were modeled using 2D finite element modeling (FEM) and the vector field was attached to the tissue model, such that tissue deformations also induced a modification of the vector field. An attractive vector field was placed around the target and repulsive ones were placed around obstacles, defining in each point of the space the desired instantaneous velocity that the needle tip should follow. Inverse kinematics was computed from the current state of the model to find the local base motion that generates the desired tip motion. This was only performed in simulation and then applied in open-loop to a real needle due to computational complexity of the FEM.

Mechanics-based models were also used with closed-loop feedback using fluoroscopic [START_REF] Glozman | Image-guided robotic flexible needle steering[END_REF] or US [START_REF] Neubach | Ultrasound-guided robot for flexible needle steering[END_REF] imaging, allowing the intra-operative steering of a flexible needle toward a target.

Reactive control using visual feedback: Visual servoing is a kind of reactive control based on visual feedback. The method computes the control inputs required to obtain a desired variations of some visual features defined directly in the acquired images. In [START_REF] Krupa | A new duty-cycling approach for 3d needle steering allowing the use of the classical visual servoing framework for targeting tasks[END_REF] and [START_REF] Chatelain | 3d ultrasoundguided robotic steering of a flexible needle via visual servoing[END_REF] it was used to control the needle trajectory using 3D US imaging and the duty-cycling method. This approach offers a great accuracy and robustness to modeling errors due to the fact that the control is directly defined in the image. It is also quite flexible since many control behaviors can be obtained depending on the design of the visual features that are chosen. For these reasons we choose visual servoing as a basis for our needle steering framework and we will describe its principles in more detail in the following section 4.3.1.

Needle steering framework

This section presents our contribution to the field of needle steering in soft tissues. We propose a generic control framework that can be adapted to control the different degrees of freedom of a robotic system holding any kind of needle shaped tool with symmetric or asymmetric tip geometry.

The proposed approach is based on visual servoing [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], which consists in controlling the system to obtain some desired variations of several features defined directly in an image, such as for example the alignment of the needle with a target in an ultrasound image. In order to offer a framework that can be adapted to many kinds of information feedback and that is not limited to visual feedback, we propose a formulation that uses the task function framework [START_REF] Samson | Robot Control: The Task Function Approach[END_REF], which is the core principle used in visual servoing. This way a single control law can be used to integrate the information on the needle and the target provided by several kinds of modalities, such as electromagnetic tracking, force feedback, medical imaging or fiber Bragg grating shape sensors.

In the following we first present the fundamentals of the task function 4.3. NEEDLE STEERING FRAMEWORK framework in section 4.3.1 and the stability aspects of the control in section 4.3.2. We then describe in section 4.3.3 how we apply this framework to the case of needle steering by using the mechanics-based needle models that we proposed in section 2.4. Finally we present in section 4.3.4 the design of several task functions that can be used in the framework to steer the needle tip toward a target while maintaining a low amount of deformations of the tissues.

Experimental validation of the framework in the case of visual feedback will be described in section 4.4.

Task function framework

A classical method used to control robotic systems is the task function framework [START_REF] Samson | Robot Control: The Task Function Approach[END_REF], that we describe in the following.

General formulation:

We consider a generic control vector v ∈ R m containing the m different input velocities that are available to control the system. This vector can typically contain the velocity of each joint of a robotic arm or the six components of the velocity screw vector of an end-effector. We note r ∈ R m the position vector associated to v, i.e. the position of the joints or the pose of the end-effector. In the task function framework, a task vector e ∈ R n is defined and contains n scalar functions that we want to control. In image-based visual servoing these tasks usually correspond to some geometrical features extracted from the images. At each instant the variations of the tasks can be expressed as

ė(t, v) = de dt = ∂e ∂t + ∂e ∂r v. (4.3)
The term ∂e ∂t represents the variations over time of the tasks that are not due to the control inputs. The tasks are linked to the control inputs by the Jacobian matrix J ∈ R n×m defined as J = ∂e ∂r .

(4.4)

Let us define ėd the desired value for the variation of the task functions. In all the following developments, the subscript . d will be used to describe the desired value of a certain quantity. The best control vector that allows fulfilling the tasks can be computed as

v = J + ėd - ∂e ∂t , (4.5)
where + stands for the Moore-Penrose pseudo-inverse operator [START_REF] Penrose | A generalized inverse for matrices[END_REF].

The variation ∂e ∂t is usually not directly available and an estimation ∂e ∂t is necessary to compute v = J + ėd -∂e ∂t . For simplicity, in the following we consider the case where ∂e ∂t = 0, which is usually associated to a static case where only the control inputs v have an action on the environment. This leads to the control law

v = J + ėd , (4.6)
which is the main control law that we will use in the experiments.

Tasks and inputs priorities: When n < m, there are more degrees of freedom (DOF) than the number of tasks to fulfill. If the tasks are independent, i.e. the rank of J is equal to the number n of tasks, then there are infinite solutions to exactly fulfill all the tasks. In this case the Moore-Penrose pseudo-inverse gives the solution with the lowest euclidean norm. If the components of the input vector are not homogeneous, for example containing both translational and rotational velocities, the euclidean norm may actually have no physical meaning. A diagonal weighting matrix M ∈ R m×m can be used in this case to give specific weights to the different components:

v = M -1 (J M ) + ėd . (4.7)
Different methods of Jacobian normalization used to tune the weights of the matrix have been summarized by Khan et al. [START_REF] Khan | Jacobian matrix normalization -a comparison of different approaches in the context of multi-objective optimization of 6-dof haptic devices[END_REF].

When n > m, there are not enough DOF to control the different tasks independently. The same thing happens if the rank of J is lower than n, meaning that some of the tasks are not independent. A diagonal weighting matrix L ∈ R n×n can then be used in these cases to give specific weights to the different tasks depending on their priority. Hence,

v = (LJ ) + L -1 ėd . (4.8)
Both weighting matrices can also be used to deal with dependant tasks in an underdetermined system, leading to the weighted pseudo-inverse [START_REF] Eldén | A weighted pseudoinverse, generalized singular values, and constrained least squares problems[END_REF] expressed as v = M -1 (LJ M ) + L -1 ėd . (4.9)

Note however that the weighted pseudo-inverse only achieves a trade-off between tasks, meaning that even high-priority tasks may not be exactly fulfilled.

NEEDLE STEERING FRAMEWORK

Hierachical stack of tasks: Absolute priority can be given to some tasks using hierarchical stack of tasks [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF]. In that case, each set of tasks with a given priority is added successively to the control output such that they do not disturb the previous tasks with higher priority. This is done by allowing the contribution of low priority tasks to lie only in the null space of the higher priority tasks. The control output that is obtained after adding the contributions of the tasks from priority level 1 to i (1 being the highest priority) is given by

v i = v i-1 + P i-1 (J i P i-1 ) + ( ėi,d -J i v i-1 ) , (4.10)
where J i is the Jacobian matrix corresponding to the task vector e i containing the tasks with priority level i, ėi,d still denotes the desired value of ėi and P i is the projector onto the null space of all tasks with priority levels from 1 to i. The projectors P i can be computed according to

P i = I m -    J 1 . . . J i    +    J 1 . . . J i    , (4.11)
where I m is the m by m identity matrix. Alternatively, these projectors can also be computed iteratively using [BB04]

P 0 = I m P i = P i-1 -(J i P i-1 ) + (J i P i-1 ) . (4.12)
For example, using only 2 priority levels, the control law thus becomes v = J + 1 ė1,d + P 1 (J 2 P 1 ) + ė2,d -J 2 J + 1 ė1,d , (4. 13) with

P 1 = I m -J 1 + J 1 .
An illustration of the hierarchical stack of tasks using this formulation can be seen in Fig. 4.2a.

Singularities: One issue when using task functions is the presence of singularities. Natural singularities may first arise when one of the tasks becomes singular, meaning that the rank of the Jacobian matrix is lower than the number n of tasks. Algorithmic singularities can also arise when tasks with different priorities become dependent, i.e. when J i P i-1 becomes singular even if J i is not. While the pseudo-inverse is stable exactly at the singularity, it leads to numerical instability around the singularity.

This numerical instability is easily illustrated using the singular value decomposition of the matrix:

J = min(n,m) i=0 σ i u i v T i , (4.14)
where the u i form an orthonormal set of vectors of R n , the v i form an orthonormal set of vectors of R m and σ i are the singular values of J . . Each E i is the set of control inputs for which the task i is fulfilled. Each S i is the input vector obtained using a single task i in the classical formulation (4.6). C is the input vector obtained using both tasks in the classical formulation (4.6). The same input vector C is obtained using the hierarchical formulation (4.13). Each R i is the input vector obtained using the singularity robust formulation (4.16) when the task i is given the highest priority. The contributions due to tasks 1 and 2 are shown with blue and red arrows, respectively, when the task 1 is given the highest priority and with green and yellow arrows, respectively, when the task 2 is given the highest priority.

The pseudo-inverse of J is then computed as

J + = min(n,m) i=0 τ i v i u T i with τ i = σ -1 i if σ i = 0 0 if σ i = 0 . (4.15)
The matrix J is singular when at least one of the σ i is equal to zero. In this case the pseudo-inverse can still be computed since it sets the value of τ i to zero instead of inverting the singular value σ i . However in practice the matrix is almost never exactly at the singularity because of numerical inaccuracies. Around the singularity, the matrix is ill conditioned and one of the σ -1 i becomes very large, leading to very large velocity outputs, which are not desirable in practice.

Algorithmic singularities can be avoided by using the singularity robust 4.3. NEEDLE STEERING FRAMEWORK formulation for the control law [START_REF] Chiaverini | Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators[END_REF]:

v i = v i-1 + P i-1 J + i ėi,d . (4.16)
While this method entirely removes algorithmic singularities, it leads to distortions of the low priority tasks, even when they are almost independent of the higher priority ones. An illustration of the hierarchical stack of tasks using this formulation can be seen in Fig. 4.2b.

In order to reduce the effect of singularities on the control outputs, damped least squares pseudo-inverse [START_REF] Deo | Overview of damped leastsquares methods for inverse kinematics of robot manipulators[END_REF] has been proposed using a different formulation of (4.15). Then

τ i = σ i σ 2 i + λ 2 , (4.17)
where λ is a damping factor. This method requires the tuning of λ and many methods have been proposed to limit the task distortions far from the singularity while providing stability near the singularity.

Stability

The task function framework is typically used to perform visual servoing, in which a visual sensor is used to provide some visual information on the system. The control of the system is performed by regulating the value of some visual features s ∈ R n , directly defined in the visual space, toward desired values s * ∈ R n . A typical approach is to design the task functions to regulate the visual features s toward the desired values with an exponential decay, such that e = ss * , (4.18) ėd = -λ s e, (4.19)

where λ s is a positive control gain that tunes the exponential decrease rate of the task vector e. In this particular case the control law (4.6) becomes v = -λ s J + e. (4.20)

In practice the real Jacobian matrix J can not be known perfectly because it depends on the real state of the system. An approximation J needs to be provided to the controller, such that the real control law becomes v = -λ s J + e. (4.21)

Using this control law, it can be shown that the system remains locally asymptotically stable as long as the matrix J J + verifies [CH06]

J J + > 0. (4.22)
Note that this stability condition is also difficult to check since the real J is not known. However this condition is usually verified in practice if the approximation J provided to the controller is not too coarse.

In the following we describe how we adapt the task function framework to the problem of needle steering and we present the method that we use to compute the estimation of the Jacobian matrix J corresponding to a given task vector.

Task Jacobian matrices

In the two previous sections we have presented the fundamentals of the task function framework. We now present how we adapt it to perform the control of a needle insertion procedure.

As was presented in section 4.1.2, we choose to use the base manipulation method to control the 6 degrees of freedom (DOF) of the needle base. The generic input vector v that was defined in the task function framework (see beginning of section 4.3.1) is thus taken as the velocity screw vector v b ∈ R 6 of the needle base, containing three translational and three rotational velocities.

We consider in this section that a task vector e ∈ R n has been defined to control the variations of a specific set of features s ∈ R n related to the needle. These features can for example consist in the position of a point along the needle shaft or the orientation of a beveled tip. The exact design of the different tasks to perform a successful needle insertion will be covered in detail in the following section 4.3.4.

In order to use the task function framework, an estimation of the Jacobian matrices associated to each task should regularly be provided during the insertion process. We propose to compute online numerical approximations of these matrices using the mechanics-based models that we defined in section 2.4. We assume that the features s can be computed from the model and we use a finite different approach to compute the numerical approximations during the insertion.

Let r ∈ SE(3) be the vector representing the current pose of the base of the needle model. We note J s ∈ R n×6 the Jacobian matrix associated to the feature vector s such that

J s = ∂s ∂r .
(4.23)

Since the state of the model is computed from r and we assume that s can be computed from the model, then s directly depends on r. The finite difference approach consists in computing the value of s taken for several poses r i spread along each direction around the current pose r. Due to the non vector space nature of SE(3), we use the exponential map Exp r to compute each r i according to

r i = Exp r (δtv i ), (4.24)
where δt is a small time step and v i ∈ R 6 is a unit velocity screw vector corresponding to one DOF of the needle base. v i represents then a translation along one axis of the base frame for i = 1, 2, 3 and v i represents a rotation around one axis of the base frame for i = 4, 5, 6. Since each v i corresponds to only one DOF of the base, each column J s,j of the Jacobian matrix (j = 1, . . . , 6) can then be approximated using the forward difference approximation

J s,j = s(r j ) -s(r) δt . (4.26)
For more accuracy, we use instead the second order central difference approximation, although it doubles the number of poses r i for which s needs to be evaluated:

J s,j = s(r j ) -s(r -j ) 2δt , (4.27)
with r -j = Exp r (-δtv j ).

(4.28)

Note that s can also lie on a manifold instead of a vector space, for example when evaluating the Jacobian corresponding to the pose of a point along the needle shaft. In this case the logarithm map Log s(r) to the tangent space should be used and leads to

J s,j = Log s(r) (s(r j )) -Log s(r) (s(r -j )) 2δt . (4.29)
Note that δt should be chosen as small as possible to obtain a good approximation of the Jacobian but not to small to avoid numerical precision issues. Now that we have defined a method to compute numerical approximations of the Jacobian matrices from our numerical needle model, we focus in the following section on the design of different task functions to control the needle insertion, i.e. the definition of s.

Task design for needle steering

An important issue to control the behavior of the needle manipulator using the task function framework is the design of the different task functions stacked in the task vector e. In this section we consider the specific case of the needle insertion procedure where a needle is held by its base. The general objectives that we want to fulfill are first the control of the needle tip trajectory to reach a target and then the control of the deformations of the needle and the tissues to avoid safety issues. The main point of the task function formulation is then to hide the complexity of the control of the base motions and to translate it into the control of some easily understandable features.

Each elementary task requires three components: the definition of the task function (see for example (4.18)), the computation of the Jacobian matrix associated to the task and the desired variation of the task function (see for example (4.19)). The difficulty is that many different task functions can be designed to fulfill a same general objective. It is also preferable that the dimension of the task vector remains as small as possible. This way we can avoid under-actuation of the system, where the tasks are not exactly fulfilled, and also decrease the probability of incompatibility between different tasks.

In the following we first cover the design of the tasks that can be used for the steering of the needle tip toward a target and we then focus on the design of the tasks that can be used to avoid tearing the tissues or breaking the needle.

Targeting tasks design

The first and main objective to fulfill in an insertion procedure is that the needle tip reaches a target in the tissues. In this section we propose different task vectors that can be used to control the tip trajectory in order to reach the target and we present their different advantages and drawbacks. We first 4.3. NEEDLE STEERING FRAMEWORK start with general task vectors that give a full control over the tip trajectory and then we successively present simpler ones that control only individual aspects of the tip trajectory. The first task vectors can be used with any kind of needle tips, while the last task vector is more specific to beveled tips.

We recall here that the subscript . d is used to define the desired value of a quantity.

Tip velocity screw control:

A first idea is to directly control the motion of the tip via its velocity screw vector v t ∈ R 6 . We denote r t ∈ SE(3) the pose of the needle tip and J tip ∈ R 6×6 the associated Jacobian matrix relative to the pose of the needle base r. The Jacobian matrix J tip is then defined such that

J tip = ∂r t ∂r ,
(4.30)

v t = J tip v b , (4.31)
where the screw vectors v t and v t are defined in their respective frames {F t } and {F b } as illustrated in Fig. 4.4. Note that J tip is computed using the finite difference method defined on a manifold that was defined by (4.29) in section 4.3.3. This Jacobian matrix will be used as a basis for the other tasks in the following since it entirely describes the relation between the motion of the needle base and the motion of the needle tip. This relation can then directly be inverted according to (4.6) to allow the control of the desired tip motion v t,d , such that

v b = J + tip v t,d . (4.32)
One advantage of this control is that it translates the control problem from the needle base to the needle tip. It can thus allow an external human operator to directly control the desired motion of the tip v t,d without having to consider the complex interaction of the flexible needle with the tissues.

However one drawback is that it constraints the six control inputs, meaning that no additional task can be added. Subsequently, in the case of the design of an autonomous control of the tip trajectory, the design of the desired variations of the tip motion v t,d should take into account its effect on the whole behavior of the needle to avoid unfeasible motions that would damage the tissues. This can thus be as difficult to design as directly controlling the motions of the needle base.

However the tip screw vector can be written as v t = v t ω t , with v t ∈ R 3 the translational velocity vector and ω t ∈ R 3 the rotational velocity vector.

In the following we propose different ways to separate the components of v t to obtain a task vector of lower dimension that is still adequate for the general targeting task and allows the addition of other task functions.

Tip velocity control:

A first solution that is better than the one proposed in previous paragraph is to limit the task vector to the control of the translational velocities v t of the tip. The corresponding Jacobian matrix is then J vt = I 3 0 3 J tip , (4.33)

where I 3 and 0 3 are the 3 by 3 identity and null matrices, respectively, and J tip was defined by (4.30). This relation can also be directly inverted according to (4.6) to allow the control of the desired tip translations v t,d , such that

v b = I 3 0 3 J tip + v t,d . (4.34)
The main advantage of this control is that it allows a direct control of the tip trajectory and keeps some free degrees of freedom (DOF) to add additional task functions. This can also easily be adapted to follow a trajectory defined by a planning algorithm or to give the control of v t,d to an external human operator.

NEEDLE STEERING FRAMEWORK

In the case of an autonomous controller, a way to reach the target is to fix the desired variations of the task vector such it goes toward the target with a fixed velocity v tip . Noting p t = x t y t z t T the position of the target in the needle tip frame {F t } (see Fig. 4.4), we have

v t,d = v tip p t p t . (4.35)
Note that this is the main targeting task design that we use in the different experiments to test our framework. In practice p t can be computed from the tracking of the needle tip and the target using any modality that allows this tracking, such as for example an imaging modality and an electromagnetic tracker.

One drawback of this task vector when it is used alone is that it does not explicitly ensure that the needle actually aligns with the target. It is thus possible that the tip translates in the direction of the target while the tip axis goes further away from the target, resulting in a motion of the needle shaft that is cutting laterally in the tissues. However, since this task vector does not constrains all the DOF of the base, an additional task function can be added to explicitly solve this issue, for example a safety task function that limits the cutting of the tissues (as will be designed later in section 4.3.4.2). Alternatively, another targeting task vector can also be designed to directly ensure that the needle aligns with the target. This is what we propose to explore in the following.

Minimal set of targeting task functions:

We propose to solve the two issues caused by the targeting task vectors that were designed in the previous paragraphs. The issue with the first task vector controlling the whole tip motion is that it constrains all the DOF of the needle base, such that no other task functions can be added. The second task vector controlling only the tip translations solves this issue but does not ensure that the needle aligns with the target.

Therefore we propose to decompose the general targeting task into the two fundamental actions that allow reaching the target: inserting the needle and orienting the needle tip axis toward the target.

Each action can be achieved using only one scalar task function. In the following we first present a task function to control the insertion of the needle and then we present two possible task functions to control the orientation of the needle tip axis.

Needle insertion:

The insertion of the needle can easily be controlled using only the velocity v t,z of the needle tip along its axis (z axis of the tip frame {F t } depicted in Fig. 4.4). The associated Jacobian matrix J vt,z ∈ R 1×6 can then be expressed as J vt,z = 0 0 1 0 0 0 J tip , (4.36)

where J tip was defined by (4.30).

The desired insertion velocity v t,z,d can then be set to a positive constant v tip (that was defined by (4.35)) during the insertion and can be set to zero once the target has been reached, such that

v t,z,d = v tip if z t > 0 0 if z t ≤ 0 , (4.37)
where we recall that z t is the distance from the tip to the target along the needle tip axis.

Needle tip axis orientation: Orienting the needle tip axis toward the target can first be achieved by minimizing the angle θ between the needle tip axis and the axis defined by the tip and the target, as illustrated in Fig. 4.4. This angle can be expressed as

θ = atan2 x 2 t + y 2 t , z t , (4.38)
where we recall that x t , y t and z t are the components of the position p t of the target in the tip frame {F t } depicted in Fig. 4.4. The Jacobian matrix J θ ∈ R 1×6 corresponding to this angle can then be derived as follows:

J θ = ∂θ ∂r = ∂θ
=   -1 0 0 0 -z t y t 0 -1 0 z t 0 -x t 0 0 -1 -y t x t 0   . (4.41)
Finally we obtain

J θ =                         - x t cos 2 (θ) z t x 2 t + y 2 t - y t cos 2 (θ) z t x 2 t + y 2 t x 2 t + y 2 t x 2 t + y 2 t + z 2 t y t x 2 t + y 2 t - x t x 2 t + y 2 t 0                         T J tip , (4.42)
where J tip was defined by (4.30).

Aligning the needle axis with the target can then be achieved by regulating the value of θ toward zero, such that θd = -λ θ θ, (4.43)

where λ θ is a positive control gain that tunes the exponential decrease rate of θ.

Alternatively the distance d between the needle tip axis and the target can also be used as a feature to minimize in order to reach the target (see Fig. 4.4). This distance can be expressed as

d = x 2 t + y 2 t .
(4.44)

The corresponding Jacobian matrix J d ∈ R 1×6 can be derived as follows: The different task functions can then be stacked together and used in (4.6), which leads to the following two possible control laws

J d = ∂d ∂r = ∂d
v b = J vt,z J θ + v t,z,d θd , (4.49) or v b = J vt,z J d + v t,z,d ḋd . (4.50)
Both control laws allow the automatic steering of the needle tip toward the target, while letting several free DOF of the needle base to perform other tasks at the same time. Note that these control laws give the same priority to both scalar tasks: different priorities could also be given by using a hierarchical formulation as presented in section 4.3.1.

Giving the control of v t,z along with θd or ḋd to an external human operator would be less intuitive than the direct control of the tip translations defined by (4.34). The exact trajectory of the tip would be harder to handle in this case due to the non-intuitive effect of θd or ḋd on the exact tip trajectory. However, it could be possible to give only the control of the insertion speed v t,z to the operator and let the system handle the alignment with the target.

Additionally, in the case of an autonomous controller using (4.37) along with (4.43) or (4.48), an adequate tuning of the insertion velocity v tip and the gain λ θ or λ d is required. If the gain is too low with respect to the insertion velocity, the needle tip does not have enough time to align with the target before it reaches the depth of the target. The gain should thus be chosen large enough to avoid mistargeting if the target is initially misaligned.

Tip-based control task functions: All the previously defined task vectors control in some way one of the lateral translations or rotations of the needle tip. They can thus be used with symmetric or asymmetric tip geometries. However, the advantage of a needle with an asymmetric tip is that the tip trajectory can also be controlled during a pure insertion using only the orientation of the asymmetry, without direct control of the lateral translations.

In the case of a beveled-tip, the lateral force created at the tip during the insertion is directly linked to the bevel orientation. Orientation of the bevel toward the target can then be achieved by regulating the angle σ around the needle axis between the target and the orientation of the bevel cutting edge (y axis), as depicted in Fig. 4.4. This angle can be expressed according to

σ = atan2(y t , x t ) - π 2 . ( 4

.51)

The corresponding Jacobian matrix J σ ∈ R 1×6 can be derived as follows:

J σ = ∂σ ∂r = ∂σ
J σ = y t d 2 - x t d 2 0 x t z t d 2 y t z t d 2 -1 J tip , (4.54)
where d was defined by (4.44).

Regulation of σ toward zero can also be achieved using

σd = -λ σ σ, (4.55)
where λ σ is a positive control gain that tunes the exponential decrease rate of σ. A smooth sliding mode control will however be preferred, as was done in [RDG + 13], to rotate the bevel as fast as possible while it is not aligned with the target. This is equivalent to define a maximum rotation velocity ω z,max in (4.55) and to use a relatively high value for λ σ , such that

σd = -ω z,max sign(σ) if |σ| ≥ ωz,max λσ -λ σ σ if |σ| < ωz,max λσ . (4.56)
Tip-based control can thus be performed by stacking this task function with the insertion velocity task function defined by (4.36) and (4.37) and use them in (4.6), which leads to the following control law

v b = J vt,z J σ + v t,z,d σd . (4.57)
This control law allows the automatic steering of the needle tip toward the target by using the asymmetry of the needle tip and also let several free DOF of the needle base to perform other tasks at the same time.

The direct control of both v t,z and σd can be given to an external human operator to perform the insertion. Alternatively, it could also be possible to give only the control of the insertion speed v t,z to the operator and let the system automatically orient the bevel toward the target.

In the case of an autonomous controller using (4.37) and (4.56), an adequate tuning of the insertion velocity v tip with respect to the rotation velocity ω z,max is necessary to ensure that the bevel can be oriented fast enough toward the target before the needle tip reaches the depth of the target. This can usually be achieved by setting a high value ω z,max [RDG + 13].

Conclusion:

We have presented several task vectors that could be used in a control law to achieve the steering of the needle tip toward a target. Each task vector uses a different strategy, such as the control of the tip velocity, the alignment of the tip axis with the target or the orientation of the asymmetry of the needle tip toward the target.

Most of the task vectors do not constrain all of the available DOF of the needle base, such that they can be used in combination with one another or with other task functions to achieve several objectives at the same time. In particular, the orientation of the bevel of a beveled-tip can be used alongside the control of the lateral translations or rotations of the tip in order to increase the targeting performances of the controller. This will be explored in the experiments presented in section 4.4.

The deformations of the needle and the tissues should also be controlled during the insertion, especially when using the control of the lateral translations of the tip, which can only be achieved by bending the needle and pushing on the tissues. Therefore, in the next section we focus on the design of additional task functions in order to ensure the safety of the insertion procedure.

Safety tasks design

The tasks defined in the previous section are used to control the trajectory of the needle tip. However they do not take into account other criteria that may be relevant to ensure a safe insertion of the needle. Two main points need to be taken into account for the safety of the insertion procedure.

First the lateral efforts exerted on the tissues should be minimized to reduce the risks of tearing, which would go against the general concept of minimally invasive procedure. The second point is to avoid breaking the needle, for obvious safety reasons. Both points can be viewed as a same objective since breaking the needle will only occur if large efforts are applied on the tissues.

In order to address these two points, we propose in the following three task functions that can be used in combination with the targeting task vectors of previous section using one of the control schemes defined in section 4.3.1 by (4.6), (4.10) or (4.16). The first task is designed toward the control of the deformations of the tissues, the second one toward the control of the deformations of the needle and the third one to achieve a trade-off between the two. An experimental comparison of the performances obtained with each task function will then be provided later in section 4.4.1.2.

Surface stretch reduction task: It can be noted that tissue tearing has the greatest probability to appear near the surface of the tissues. Indeed, this occurs when the skin has already been fragilized by the initial cut of the needle and when less surrounding tissues are present to maintain their cohesion.

A first solution to avoid tearing the surface of the tissues is to ensure that the body of the needle remains close to the initial position of the insertion point. This can be achieved by reducing the relative lateral position δ ∈ R 2 on the tissue surface between the current position of the needle c N (L f ree ) and the initial position of the insertion point c T (0), as illustrated in Fig. 4.5 (note that we choose to take the notations c N , c T and L f ree that were introduced in the definition of the two-body model in section 2.4.2).

This task and the associated Jacobian matrix can then be expressed according to

δ = P s c N (L f ree ) -c T (0) , (4.58
)

J δ = P s ∂c N (L f ree ) ∂r = P s J L f ree , (4.59)
where P s ∈ R 2×3 is an orthogonal projector onto the tissue surface and Figure 4.5: Illustration of the geometric features used for the safety task functions. Note that the representation is here limited to a 2D case, however in the general case the angle γ is defined in the plane containing the needle base axis z and the initial insertion point c T (0).

J L f ree ∈ R 3×6
is the Jacobian matrix linking the variations of the position of the needle point at the curvilinear coordinate L f ree to the variations of the needle base pose r. This matrix J L f ree is computed from the model using the method described by (4.27) in section 4.3.3. Regulation of δ toward zero can be achieved using the classical law

δd = -λ δ δ, (4.60)
where λ δ is a positive control gain that tunes the exponential decrease rate of δ. Alternatively the scalar distance δ can also be directly used to decrease 143 CHAPTER 4. NEEDLE STEERING the dimension of the task:

δ = δ , (4.61
)

J δ = δ T δ J δ , (4.62) δd = -λ δ δ, (4.63)
where λ δ is a positive control gain that tunes the exponential decrease rate of δ. Note that this formulation introduces a singularity when δ = 0. However, it can be shown that the local asymptotic stability of the system remains still valid [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]. From a numerical point of view, it can also be noted that δ T δ is always a unit vector for δ = 0, such that J δ does not introduce arbitrary large values near δ = 0. Therefore in the following we will use the scalar version of the task for the reduction of the tissue stretch at the surface.

Needle bending reduction task: A solution to avoid breaking the needle is to ensure that the needle remains as straight as possible. However maintaining the needle strictly straight is not possible since needle bending is necessary to steer the needle tip, either from lateral base motion or by using the natural curvature at the needle tip. We propose to use the bending energy of the needle as a quantity to minimize. This energy can be computed from the needle models presented in section 2.4. As defined in (2.19), the energy is given by

E N = EI 2 L N 0 d 2 c N (l) dl 2 2 dl,
where we recall that E is the Young's modulus of the needle, I is the second moment of area of the needle section and c N is the spline curve of length L N representing the shape of the needle.

The corresponding Jacobian matrix J E N ∈ R 1×6 can then be computed from the model using the method described by (4.27) in section 4.3.3

J E N = ∂E N ∂r .
(4.64)

Regulation of E N toward zero can by achieved using the classical law

ĖN,d = -λ E N E N , (4.65)
where λ E N is a positive control gain that tunes the exponential decrease rate of E N .

NEEDLE STEERING FRAMEWORK

Needle base alignment task: Limiting the distance between the needle and the initial position of the insertion point does not ensure that the needle is not bending outside of the tissues. Similarly, once the needle has been inserted, reducing the bending of the needle does not ensure that the needle is not pushing laterally on the surface of the tissues. In order to avoid pushing on the tissues near the insertion point and to also limit the bending of the needle outside the tissues, the needle base axis can be maintained oriented toward the insertion point. This can be viewed as a remote-center-of-motion around the initial insertion point, in the case where this one is not moving, i.e. if no external tissue motions occur.

We propose to achieve this goal by regulating toward zero the angle γ between the needle base z axis and the initial location of the insertion point c T (0), as illustrated in Fig. 4.5. This way the needle base axis should also follow the insertion point in the case of tissue motions that are not due to the interaction with the needle. Noting x 0 , y 0 , and z 0 the coordinates of the initial position of the insertion point c T (0) in the needle base frame {F b } (see Fig. 4.5), the angle γ can be expressed according to γ = atan2

x 2 0 + y 2 0 , z 0 . (4.66)

The Jacobian matrix J γ ∈ R 1×6 corresponding to this angle can then be derived as follows:

J γ = ∂γ ∂r = ∂γ ∂c T (0) ∂c T (0) ∂r , (4.67) with ∂γ ∂c T (0) = x 0 cos 2 (γ) z 0 x 2 0 + y 2 0 y 0 cos 2 (γ) z 0 x 2 0 + y 2 0 - x 2 0 + y 2 0 x 2 0 + y 2 0 + z 2 0 (4.68) and ∂c T (0) ∂r =   -1 0 0 0 -z 0 y 0 0 -1 0 z 0 0 -x 0 0 0 -1 -y 0 x 0 0   . (4.69)
Finally we obtain

J γ =                         - x 0 cos 2 (γ) z 0 x 2 0 + y 2 0 - y 0 cos 2 (γ) z 0 x 2 0 + y 2 0 x 2 0 + y 2 0 x 2 0 + y 2 0 + z 2 0 y 0 x 2 0 + y 2 0 - x 0 x 2 0 + y 2 0 0                         T . (4.70)
Regulation of γ toward zero can by achieved using the classical law

γd = -λ γ γ, (4.71)
where λ γ is a positive control gain that tunes the exponential decrease rate of γ.

Conclusion:

In this section we have defined three different task functions that can be used to control the deformations of the needle or tissues during the insertion. These task functions can be combined together with a targeting task using the task function framework in order to obtain a final control law that allows reaching a target with the needle tip while ensuring the safety aspect of the insertion procedure.

In the following section we propose to test in different experimental scenarios the whole needle steering framework that we designed. Several combinations of the task vectors defined in sections 4.3.4.1 and 4.3.4.2 will be explored as well as the different formulations used to fuse them into one control law as described in section 4.3.1.

Framework validation

In this section we present an overview of the experiments that we conducted to test and validate our proposed needle steering framework. We first use the stereo cameras to obtain a reliable feedback on the needle localization in order to test the different aspects of the framework independently from the quality of the tracking. We then perform insertions under 3D ultrasound visual guidance using the tracking algorithm that we proposed in chapter 3.

Insertion under camera feedback

In this section we propose to evaluate the performances of our framework when using the visual feedback provided by the stereo camera system presented in section 1.5.2. In all the experiments the stereo camera system is registered and used to retrieve the position of the needle shaft in the tissues using the registration and tracking methods described in section 3.4.1.

We first present experiments that we performed to combine our framework with the duty-cycling control technique described in section 4.1.1. We then compare the performances obtained during the needle insertion when using the different safety task functions that were defined in section 4.3.4.2. Finally we propose to test the robustness of the method to modeling errors introduced by lateral motions of the tissues.

Switching base manipulation and duty-cycling

We first propose to use both base manipulation and tip-based control to insert a needle and reach a virtual target. Tip-based control allows a fine control of the tip trajectory, however the amplitude of the lateral tip motions that can be obtained is limited, such that the target can be unreachable if it is not initially aligned with the needle axis. On the contrary, using base manipulation allows a better control over the lateral tip motions at the beginning of the insertion, however the effect of base motions on the tip motions is reduced once the needle tip is inserted deeper in the tissues.

In the following we use an hybrid controller that alternates between dutycycling control (see section 4.1.1), when the target is almost aligned with the needle, and base manipulation using our task framework (see section 4.3) in order to accurately reach a target that may be misaligned at the beginning of the insertion.

Experimental conditions (setup in France): In these experiments, the Angiotech biopsy needle is actuated by the Viper s650. The insertion is done in a gelatin phantom embedded in a stationary transparent plastic container. Visual feedback is obtained using the stereo cameras system and the whole needle shaft is tracked in real-time by the image processing algorithm described in section 3.4.1. A picture of the setup is shown in Fig. 4.6.

A virtual target to reach is defined just before the beginning of the insertion such that it is located at a predefined position in the initial tip frame. We use the virtual springs model presented in section 2.4.1 with polynomial needle segments of order r = 3. The stiffness per unit length of the model is set to 10000 N.m -2 for these experiments and the length threshold to add a new virtual spring is set to L thres = 2.5 mm. The rest position of a newly added spring (defined as p 0,i in section 2.4.1, see Fig. 2.5) is set at the position of the tracked needle tip in order to compensate for modeling errors. This is similar to the update method 3 presented in section 3.6.2. The pose of the needle base of the model is updated using the odometry of the robot.

Control:

We use either base manipulation using the task function framework or duty-cycling control depending on the alignment of the target with the needle tip axis. Duty-cycling is used when the target is almost aligned and only small modifications of the tip trajectory are needed. Base manipulation is used when larger tip motions are necessary to align the needle with the target.

Base manipulation control: We use three tasks to control the needle manipulator and we fuse them using the singularity robust formulation of the task function framework, as defined by (4.16) in section 4.3.1. Each task is given a different priority level such that it does not disturb the tasks with higher priority. The tasks are defined as follows.

• The first task with highest priority controls the tip translation velocity v t , as defined by (4.33) and (4.35). We set the insertion velocity to 1 mm.s -1 . Note that we choose this task over the tip alignment tasks defined in (4.49) and (4.50) because it does not require the tuning of an additional gain.

• The second task with medium priority controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal 4.4. FRAMEWORK VALIDATION rotation speed ω z,max is set to 60 • .s -1 and the gain λ σ is set to 4 3 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 45 • .

• The third task with lowest priority is used to reduce the mean deformations of the tissues δ m , which we compute here from the virtual springs interaction model according to

δ m = 1 L ins n i=1 l i δ i , (4.72)
where L ins is the current length of the needle that is inserted in the tissues, n is the current number of virtual springs, δ i is the distance between the needle and the rest position of the i th virtual spring, i.e. the virtual spring elongation, and l i is the length of the needle model that is supported by this virtual spring. The Jacobian matrix J δm corresponding to δ m is numerically computed from the model using the method described by (4.27) and the desired variation of δ m is computed as

δm,d = -λ δm δ m , (4.73)
with the control gain λ δm set to 1.

The final velocity screw vector v b applied to the needle base is then computed according to

v b = J + vt v t,d + P 1 J + σ σd -λ δm P 2 J + δm δ m , (4.74)
with P 1 = I 6 -J + vt J vt , (4.75)

P 2 = I 6 - J vt J σ + J vt J σ = P 1 -(J σ P 1 ) + (J σ P 1 ) , (4.76)
where I 6 is the 6 by 6 identity matrix.

Duty-cycling control:

We use duty-cycling control when the target is almost aligned with the needle tip axis. This is detected by comparing the angle θ between the target and the tip axis (as defined in (4.38)) with the maximum angle θ DC obtained during one cycle of duty-cycling. This angle corresponds to the angle obtained during a cycle with only insertion (dutycycle ratio DC = 0), such that

θ DC = K nat L DC . (4.77)
where K nat is the natural curvature of the needle tip trajectory during the insertion and L DC is the total insertion length of a cycle, set to 3 mm in this experiment. If θ < θ DC , the needle would overshoot the current desired direction in less than a cycle length. In that case it is better to reduce the effective curvature K ef f of the tip trajectory such that it aligns with the desired direction, i.e using

K ef f = θ L DC , (4.78) DC = 1 - θ L DC K nat (4.79)
The total rotation of the needle during each rotation phase is set to 2π + σ, where σ is the angle between the target and the bevel as defined in (4.51), such that the bevel is oriented in the target direction before starting the translation phase.

Experimental scenarios: Four experiments are performed with a same phantom to validate our method. At the beginning of each experiment, the needle is placed such that it is normal to the surface of the gelatin and its tip slightly touches it. The insertion point is shifted between the experiments in a way that the needle can not cross a previous insertion path. The needle is first inserted 7 mm in the gelatin to allow the manual initialization of the tracking algorithm in the images. Then the insertion procedure starts with an insertion speed of 1 mm.s -1 and is stopped when the target is no more in front of the needle tip.

Open-loop insertion toward an aligned target: In the first experiment, a virtual target is defined before the beginning of the insertion such that it is aligned with the needle and placed at a distance of 8 cm from the tip. A straight insertion along the needle axis is then performed in openloop control. Fig. 4.7a shows the view of the front camera at the end of the experiment and Fig. 4.8a shows the 3D lateral distance between the needle tip axis and the target. Note that the measure presents a high level of noise at the beginning of the insertion. This is first due to the noisy estimation of the needle direction at the beginning of the insertion since the visible part of the needle is small. Second, the needle tip is far from the target at the beginning of the insertion, which amplifies the effect of the direction error on the lateral distance. We can see that the target is missed laterally by 8 mm at the end because of the natural deflection of the needle. This experiment justifies that needle steering is necessary to accurately reach a target even if it is correctly aligned with the needle axis at the beginning of the procedure.

Tip-based control with a misaligned target: In a second experiment, the target is shifted 1 cm away from the initial tip axis and such that a 135 • rotation is necessary to align the bevel toward the target. The (b) Duty-cycling control with a target shifted 1 cm away from the initial needle axis: duty-cycling control is saturated and the target is missed due to insufficient tip deflection. The target can be reached in both cases using the hybrid control framework ((c) aligned target and (d) shifted target). In each graph, the purple sections marked "DC" correspond to duty-cycling control and the red sections marked "BM" correspond to base manipulation.

duty-cycling control is used alone for this experiment. Fig. 4.7b shows the view of the front camera at the end of the experiment and Fig. 4.8b shows the 3D lateral distance between the needle tip axis and the target. After the first rotation, the duty-cycling controller is saturated and only performs pure insertion phases. We can see that the lateral alignment error decreases during the insertion. However the natural curvature of the needle is not Table 4.1: Final lateral position error between the needle tip and the target for different insertion scenarios. Final lateral error (mm) Straight insertion and aligned target 7.6 Duty-cycling and shifted target 4.9 Hybrid control and aligned target 0.6 Hybrid control and shifted target 0.1 sufficient to compensate for the initial error and the target is finally missed by 5 mm. This experiment justifies that base manipulation is necessary to accurately reach a misaligned target with a standard needle or that a needle offering higher curvature needs to be used.

Hybrid control: Two other experiments were performed with the same initial target placements (one aligned target and one misaligned target) and using the hybrid controller with both base manipulation and duty-cycling. Figure 4.7c and 4.7d show the view of the front camera at the end of the experiments and Fig. 4.8c and 4.8d show the 3D lateral distance between the needle tip axis and the target. We can see that the controller allows reaching the target with a sub-millimeter accuracy in both cases.

Table 4.1 shows a summary of the final lateral targeting error between the tip and the target. The targeting error along the needle direction was under 0.25 mm in each experiment, which corresponds to the accuracy of the vision system. These experiments show that using base manipulation in addition to tip-based steering allows a larger reachable space compared to the sole use of tip-based control methods.

In addition, we can observe that the controller rarely switched to dutycycling, as can be seen in Fig. 4.8c and Fig. 4.8d. This is due to the small natural curvature of the needle tip trajectory obtained for this association of needle and phantom. In this case it may not be necessary to reduce the curvature of the needle tip trajectory. We could just orient the bevel edge toward the target, as is done by the second task of the base manipulation controller, and avoid the high number of rotations required to perform dutycycling control. However duty-cycling control should still be used when using a more flexible needle, and more especially if the tip trajectory is defined by a planning algorithm that allows non-natural curvatures.

A second observation concerns the oscillations in the lateral error that appear during the duty-cycling control in Fig. 4.8c and 4.8d. These oscillations are due to a small misalignment between the axis of rotation of the robot and the actual axis of the needle. This misalignment introduces some lateral motions of the needle during the rotation phases, which in turn modify the needle tip trajectory. From a design point of view, it shows that the accuracy of the realization of a needle steering mechanical system can have a direct effect on the accuracy of the needle steering. Furthermore, depending on the frame in which the observation is made, a lateral motion of the needle base can be seen as a motion of the phantom, so that this oscillation phenomenon confirms the fact that tissue motions is an important issue for an open loop insertion procedure. This effect is likely to have a greater importance when using relatively stiff needles, for which base motion have a significant effect on the tip motion. On the contrary it should have a lower impact when using more flexible needles, so that duty-cycling control is better suited for very flexible needles.

Conclusion:

We have seen that combining both base manipulation and tip-based control during a visual guided robotic insertion allows a good targeting accuracy in a large reachable space. This validate the fact that using additional degrees of freedom of the needle base can be necessary to ensure the accurate steering of the needle tip toward a target.

We also observed that duty-cycling control was actually not really adapted to our experimental setup due to the low natural curvature of the tip trajectory during the insertion. Therefore in the following we do not use dutycycling control anymore but we only orient the cutting edge of the bevel toward the target, such that the full curvature of the needle tip trajectory is used.

As a final note, we observed during the experiments that the third task used to reduce the deformations of the tissues had almost no influence on the final velocity applied to the needle base. This is due to the singularity robust formulation and the fact that the task is near incompatible with the first task with high priority. The contribution of the third task was then greatly reduced after the projection in the null space of the first task. Therefore we do not use this formulation in the followings and we use instead the classical formulations defined by (4.6) or (4.10).

Safety task comparison

We propose here to compare the performances obtained when using the different safety tasks that we defined in section 4.3.4.2.

Experimental conditions (setup in France): In these experiments, the Angiotech biopsy needle is actuated by the Viper s650. The insertion is done in a gelatin phantom embedded in a stationary transparent plastic container. Visual feedback is obtained using the stereo cameras system and the whole needle shaft is tracked in real-time by the image processing algorithm described in section 3.4.1. A raisin is embedded in the gelatin 9 cm under the surface and used as a real target. A picture of the setup is shown in Fig. 4.9. We use the two-body model presented in section 2.4.2 with polynomial needle segments of order r = 3. We fix the length of the needle segments to 1 cm, resulting in a total of n = 13 segments and the last segment measuring 0.6 mm. A soft phantom is used in these experiments, such that the stiffness per unit length of the model is set to 1000 N.m -2 . The length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm. The pose of the needle base of the model is updated using the odometry of the robot.

Control: We use three tasks for the control of the needle manipulator and we fuse them using the classical formulation of the task function framework, as defined by (4.6) in section 4.3.1. The different tasks are defined as follows.

• The first task controls the tip translation velocity v t , as defined by (4.33) and (4.35). We set the insertion velocity v tip to 5 mm.s -1 .

• The second task controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal rotation speed ω z,max is set to 60 • .s -1 and the gain λ σ is set to 10 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 6 • .

• The third task is one of the three safety tasks defined in section 4.3.4.2: reduction of the tissue stretch δ at the surface ((4.61), (4.62) and (4.63)), reduction of the needle bending energy E N ((2.19), (4.64) and (4.65)) or reduction of the angle γ between the needle base axis and the insertion point ((4.66), (4.70) and (4.71)). The control gain for each of these tasks (λ δ , λ E N or λ γ ) is set to 1.

We observed in the previous section that the singularity robust hierarchical formulation (see (4.16)) induces too much distortion of the low priority tasks. Therefore, we choose here to give the same priority level to each task, such that the control should give a trade-off between good targeting and safety of the procedure. The final velocity screw vector applied to the needle base v b is then computed according to

v b =   J vt J σ J 3   +   v t,d σd ė3,d   , (4.80)
where J 3 is the Jacobian matrix corresponding to the safety task (either J δ , J E N or J γ ) and ė3,d is the desired variation for the safety task (either δd , ĖN,d or γd ). Note that the total task vector is here of dimension 5 while we have 6 degrees of freedom available, such that all tasks should ideally be fulfilled.

For the computation of the control law, the desired variations of the two first tasks are computed from the measures of the target and tip position using the visual feedback. On the contrary, the desired variations of the safety tasks are computed from the needle interaction model. Experimental scenarios: Five insertions are performed for each kind of safety task. The needle is placed perpendicular to the tissue surface before the beginning of the insertion. Initial insertion locations are chosen such that they are sufficiently far away from the previous insertions, leading to an initial misalignment with the target up to 1.7 cm. The needle is first inserted 1 cm into the phantom to manually initialize the tracking algorithm. The controller is stopped once the needle tip reaches the depth of the target. Pictures of the initial and final state of one experiment are shown in Fig. 4.10. In the following we compare the values taken by each of the three physical quantities defined for the third task during the experiments, namely the tissue stretch at the surface, the needle bending energy and the angle between the needle base axis and the insertion point. These values are recorded from the state of the model during the insertions.

Targeting: Let us first look at the targeting performances of the method. The lateral distance between the needle tip axis and the target was measured during the insertions and is shown in Fig. 4.11. As stated previously this measure is noisy at the beginning of the insertion due to the distance between the needle tip and the target. The mean value of the final lateral targeting error across the five insertion procedures is summarized in Fig. 4.12.

The target could be reached in all cases with an accuracy of less than 2 mm, which is sufficiently accurate for most clinical needle insertion applications. This demonstrates the good performances of our steering method. Similar targeting performances are obtained when reducing the surface tissue stretch or reducing the needle bending energy. Aligning the needle base with the insertion point further decreases the targeting error. However this result should be interpreted with caution and may be due to statistical variance, as the targeting error is indeed close to the diameter of the needle (0.7 mm) and the visual system accuracy (0.25 mm).

Surface tissue stretch: Let us now consider the effect of the tasks on the tissue stretch δ at the surface. The value of δ for each experiment is shown in Fig. 4.13. The mean value of δ over time and across the five insertion procedures is summarized in Fig. 4.14 for each active task. As expected, actively reducing the surface stretch effectively reduces the surface stretch compared to the other safety tasks. On the other hand reducing the bending of the needle introduces a higher stress to the tissue surface. This can be explained by the fact that keeping the needle straight outside of the tissues requires that the internal shear force applied to the needle at the tissue surface is small. This is only possible if the integral of the force load applied to the needle in the tissues is near zero. Since the needle tip needs to be steered laterally to reach the target, some load is applied to the tissues near the tip. An opposing load is thus necessary near the tissue surface to drive the integral of the load to zero, leading to a deviation of the needle shaft from the initial position of the insertion point.

An intermediate between these two behaviors seems to be obtained when aligning the needle with the initial position of the insertion point. This could be expected since orienting the needle base tends to move the needle body toward the same direction, i.e. toward the insertion point, hence reducing the surface stretch. However bending of the needle outside of the tissues is still possible due to the interaction with the tissues, creating a certain amount of stretch at the surface.

Needle bending energy: Let us now look at the effect of the tasks on the bending energy E N stored in the needle. The value of E N for each experiment is shown in logarithm scale in Fig. 4.15. The mean value of E N over time and across the five insertion procedures is summarized in Fig. 4. 16 for each active task.

As expected, actively reducing the bending energy effectively reduces the energy compared to the other safety tasks. On the other hand reducing the tissue stretch at the surface requires a higher needle bending. This can be explained by the fact that steering the needle tip laterally while keeping the needle near the insertion point results in a force load applied by the tissues only on one side of the needle. Needle bending outside of the tissues is thus necessary to be able to obtain this load.

As seen previously for the surface tissue stretch, aligning the needle with the initial position of the insertion point seems to provide an intermediate between these two behaviors. This could be expected since orienting the needle base axis toward the insertion point tends to straighten the part of the needle that is outside of the tissues, hence reducing the overall bending energy. However the needle can still bend near the surface and inside the tissues, which is good to perform the targeting task.

An additional observation can be made on the behavior of the needle bending reduction task. Once the needle has been inserted in the tissues and some natural deflection appeared, moving the needle base only provides a limited way of changing the shape of the needle inside the tissues. This creates a non-zero floor value under which the bending energy cannot be reduced without removing the needle from the tissues. From the task function point of view, when the floor value is reached the corresponding task Jacobian matrix becomes incompatible with the task controlling the insertion. A singularity occurs in this case, leading to some instabilities that increases the needle bending, as could be observed in some experiments (for example the blue curve in Fig. 4.15b). This behavior indicates that using this task is not suitable to increase the safety of the control.

Base axis insertion point angle: Let us finally consider the effect of the tasks on the angle γ between the needle base axis and the initial position of the insertion point. The value of γ for each experiment is shown in Fig. 4.17. The mean value of γ over time and across the five insertion procedures is summarized in Fig. 4.18 for each active task.

As expected, actively reducing the angle between the base axis and the insertion point effectively reduces this alignment error when compared to the other safety tasks. As discussed previously, reducing the tissue stretch at the surface requires bending the part of the needle that is outside the tissues to fulfill the targeting task. Since the needle is constrained to pass by the initial position of the insertion point, this bending can only be achieved by rotating the needle base to put it out of alignment, resulting in a higher value of γ. Similarly, we have seen that reducing the bending of the needle introduces a stretch of the tissues at the surface to achieve the targeting task. Since the needle body is aligned with the needle base axis due to the reduced bending, then the base can not be aligned with the insertion point.

It can also be observed during all the experiments that the features associated to the safety tasks tend to increase near the end of the insertion, as visible in Fig. 4.13a and 4.17c. A small increase of the lateral distance near the end can also be observed in Fig 4 .11. Since the task functions are designed to regulate these features toward zero, this effect indicates an incompatibility between the safety and targeting tasks. The total Jacobian matrix defined in (4.80) is then close to singularity, such that the computation of the pseudo-inverse introduces some distortions. The hierarchical formulation (4.10) of the task function framework could be used instead of the classical formulation (4.6) to choose which task should have the priority in this case. This point will be explored later in section 4.4.2.

Conclusion:

Through these experiments we have confirmed that steering a flexible needle in soft tissues requires a certain amount of tissue deformations and needle bending. Trying to steer the needle while actively reducing the deformations at the surface of the tissues can only be achieved by bending the needle. Trying to reduce the amount of bending during the steering can only be achieved through deformations of the tissue surface. Keeping the needle base aligned with the initial position of the insertion point seems to allow needle steering while procuring a trade-off between tissue deformations near the surface and needle bending outside the tissues.

In conclusion, the last method should be preferred in general to reduce both the needle and the tissues deformations. The task reducing the tissue stretch at the surface can be used if the needle is not too flexible, such that it does not bend too much outside of the tissues. On the contrary, the task reducing the needle bending should be avoided since it introduces some stability issues in addition to the deformations of the tissues.

Robustness to modeling errors

We now propose to evaluate the robustness of the base manipulation framework towards modeling errors and tissue motions.

Experimental conditions (setup in France): In these experiments, the Angiotech biopsy needle is actuated by the Viper s650. The insertion is done in a gelatin phantom embedded in a transparent plastic container. The phantom is moved manually during the first half of the insertion. Visual feedback is obtained using the stereo camera system and the whole needle shaft is tracked in real-time by the image processing algorithm described in section 3.4.1. The setup is similar to the previous section and can be seen in Fig. 4.9.

A virtual target is defined just before the beginning of the insertion such that it is 8 cm under the tissue surface and 4 mm away from the initial needle axis. This target is fixed in space and does not follow the motions applied to the phantom, hence simulating a moving target from the point of view of the needle which is embedded in the phantom.

We use the two-body model presented in section 2.4.2 with polynomial needle segments of order r = 3. We fix the length of the needle segments to 1 cm, resulting in a total of n = 13 segments and the last segment measuring 0.6 mm. The stiffness per unit length of the model is set to 3200 N.m -2 and the length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm.

Control:

We use two tasks for the control of the needle manipulator and we fuse them using the classical formulation of the task function framework, as defined by (4.6) in section 4.3.1. The tasks are defined as follows.

• The first task controls the tip translation velocity v t , as defined by (4.33) and (4.35). We set the insertion velocity v tip to 2 mm.s -1 .

• The second task controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal rotation speed ω z,max is set to 60 • .s -1 and the gain λ σ is set to 10 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 6 • .

The final velocity screw vector applied to the needle base v b is then computed according to

v b = J vt J σ + v t,d σd . (4.81)
The controller is stopped once the needle tip reaches the depth of the target.

Experimental scenarios: We perform four insertions using the controller defined previously. For each experiment, the phantom is manually moved laterally with respect to the insertion direction with an amplitude of up to 1 cm. During two of the insertions, the interaction model is updated using only the pose of the needle manipulator. During the two other insertions, the model is also updated with the UKF-based update algorithm defined in section 3.5. We use the position feedback version of the algorithm by measuring the position of needle points separated by 5 mm along the needle shaft. The process noise covariance matrix is set with diagonal elements equal to 10 -8 m 2 and the noise covariance matrix with diagonal elements equal to (2.5 × 10 -4 ) 2 m 2 .

Results:

The lateral distance between the needle tip axis and the target is shown in Fig. 4.19, either measured using the needle tracking (Fig. 4.19a) or estimated from the needle model (Fig. 4.19b). An example of the final state of two models, one updated and one not updated, during a single insertion is shown in Fig. 4.20. We can see that when the position of the tissue model is not updated, the needle model does not fit to the real needle. However the target can be reached with sub-millimeter accuracy in all cases, despite the fact that an inaccurate model is used in some cases. This shows that an accurate modeling of the current state of the insertion is not necessary to obtain estimates of the Jacobian matrices which can maintain the convergence of the control law, as previously expressed by (4.22). The task controller proves to be robust to modeling uncertainties thanks to the closed-loop feedback compensating for the errors appearing in the Jacobian matrices. Nevertheless it can be noted that updating the model is necessary if this one must be used for prediction of the needle tip trajectory.

Furthermore, the fact that the phantom is moving while the target is not moving introduces an apparent motion of the target with respect to the needle tip. The designed targeting tasks shows good targeting performances by compensating for this target motion thanks to the closed-loop nature of the control.

Conclusions: From these results, we have good reasons to expect good targeting performances when using 3D ultrasound (US) volume as feedback, even if the probe pose is not accurately estimated and causes the model to Yellow and blue lines are, respectively, the needle and tissue spline curves of a model updated using only the pose feedback of the needle manipulator, such that the position of the tissue spline (blue) is not updated during the insertion. Red and green lines are, respectively, the needle and tissue spline curves of a model updated using the pose feedback of the needle manipulator and the visual feedback, such that the position of the tissue spline (green) is updated during the insertion.

be updated from inaccurate measures. The close-loop control may be able to ensure good targeting as long as the desired values for the tasks are computed in the same image space, i.e. both needle and target are detected using the same US volume. In the following section we present additional experiments to see if this intuition can be confirmed.

Insertion under US guidance

In previous sections we tested our steering framework using cameras to track the needle in a translucent phantom. If cameras offer a good accuracy, in clinical practice the needle is inserted in opaque tissues, making cameras unusable for such procedure. In this section we propose to test if the framework can be used in practice using a clinically relevant imaging modality.

We present experiments performed using 3D ultrasound (US) as the visual feedback to obtain the 3D position of the body of the needle. We mainly focus our study on the targeting accuracy and also consider the effect of setting different priority levels for the different tasks.

Experimental conditions (setup in France): In these experiments, the Angiotech biopsy needle is actuated by the Viper s650. Two phantoms are used, one gelatin phantom and one phantom with a porcine liver embedded in gelatin. We use the 3D US probe and station from BK Ultrasound to grab online 3D US volumes. The US probe is fixed to the end effector of the Viper s850 and maintained fixed in contact with the phantom. The needle is inserted from the top of the phantom while the probe is set to the side of the phantom, as illustrated in Fig. 4.21. A thin plastic film is set to replace one side of the plastic container, allowing a soft contact between the probe and the phantom such that the US waves can propagate through the phantom. This ensures a good visibility of the needle in the US volume, by avoiding too much reflection of the US wave outside of the transducer. This orthogonal configuration can be observed in practice in several medical applications, such as kidney biopsy or prostate brachytherapy, where the needle is inserted perpendicularly to the US wave propagation direction.

The whole needle shaft is tracked in each volume using the tracking algorithm described in section 3.4.2. A virtual target is manually defined before the beginning of the insertion.

The acquisition parameters of the US probe are set to acquire 31 frames during a sweeping motion with an angle of 1.46 • between successive frames. The acquisition depth is set to 15 cm, resulting in the acquisition of one volume every 900 ms. The needle is around 4 cm from the probe transducer for each experiment, which leads to a maximum resolution of 0.85 mm in the insertion direction and 0.3 mm × 1.72 mm in the other lateral directions. A focus length of 5 cm is set for the transducer to obtain a good effective resolution near the needle.

We use the two-body model with polynomial needle segments of order r = 3. We fix the length of the needle segments to 1 cm, resulting in a total of n = 13 segments and the last segment measuring 0.6 mm. The stiffness per unit length of the model is set to 1000 N.m -2 and the length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm.

Control:

We use three tasks for the control of the needle manipulator and we fuse them using the hierarchical formulation of the task function framework, as defined by (4.10) in section 4.3.1. Each task is given a different priority level such that it does not disturb the tasks with higher priority. The tasks are defined as follows.

• The first task controls the tip translation velocity v t , as defined by (4.33) and (4.35). We set the insertion velocity v tip to 1 mm.s -1 .

to the needle base is then computed according to

v b = J vt J σ + v t,d σd + P 1 (J δ P 1 ) + δd -J δ J vt J σ + v t,d σd , (4.82)
with

P 1 = I 6 - J vt J σ + J vt J σ , (4.83)
where I 6 is the 6 by 6 identity matrix.

In the second set the safety task has the highest priority and the two targeting tasks have the same lower priority. The final velocity screw vector v b is then computed according to

v b = J + δ δd + P 2 J vt J σ P 2 + v t,d σd - J vt J σ J + δ δd , (4.84)
with P 2 = I 6 -J + δ J δ . (4.85)

Experimental scenario: Four insertions are performed in the gelatin phantom and four insertions in the porcine liver embedded in gelatin. For each type of phantom two insertions are performed using a higher priority for the targeting tasks as defined by (4.82) and two insertions are performed using a higher priority for the safety task as defined by (4.84).

For each experiment, the needle is first placed perpendicular to the surface of the phantom with its tip slightly touching the surface. This position allows the initialization of the needle model and the tissue surface model using the current pose of the needle holder. The needle is then inserted 1.5 cm in the tissues and a 3D US volume is acquired. The needle tracking algorithm is initialized by manually segmenting the insertion point and the needle tip in the volume. A virtual target point is manually chosen in the volume between 5 cm and 10 cm under the needle.

The pose of the probe is initialized separately for each experiment using the registration method described in section 3.6.3. The needle tracking algorithm defined in section 3.4.2 is also initialized at the same time. Then the chosen control law is launched and stops when the tip of the tracked needle reaches the depth of the target.

Results: We first discuss the targeting performances obtained for the different experiments and then we discuss the effect of the priority order on the realization of the safety task. We can see that the target can be reached in each case with a final lateral targeting error below 3 mm, which comes close to the maximal accuracy of the reconstructed US volumes. This accuracy may be sufficient for most clinical applications. However, for more demanding applications, using a better resolution for the US volume acquisition could be sufficient to achieve a better targeting accuracy. The priority order does not seem to have a significant impact on the final targeting error, although slightly larger errors could be observed for the insertions in gelatin when the safety task was set to the highest priority.

FRAMEWORK VALIDATION

During these experiments we choose to update the needle model using only the pose feedback of the needle manipulator; no update of the position of the tissue spline is used to compensate for the modeling errors introduced by the constant stiffness per unit length set in the model. This confirms that the targeting performances are quite robust to modeling approximations.

The registration of the probe pose performed at the beginning of the insertion is also quite inaccurate, especially concerning the orientation of the probe. Indeed, it depends on the quality of the manual segmentation of the part of the needle that is initially visible in the US volume. Since this needle part is initially short and the resolution of the volume is limited, it is difficult to manually segment the correct orientation of the needle. Nevertheless, since the inputs of the targeting tasks are provided using directly the position of the target in the frame of the needle tip tracked in the volume, the target can still be accurately reached. This way these experiments have demonstrated that the exact pose of the probe is not required by the steering framework to achieve good targeting performances, thanks to the closed-loop nature of the control.

Safety task performances: Let us now look at the safety task that was added to minimize the tissue deformations at the surface. The placement of the probe on the side of the phantom is such that the top surface of the tissues is visible in the US volumes. Hence we can measure the stretch at the surface of the tissues during the insertions. The initial position of the insertion point is recorded at the initialization of the needle tracking algorithm. The surface stretch is then measured as the distance between this initial position and the current position of the tracked needle at the surface.

The measured surface stretch during the insertions is shown in Fig. 4.24 along with the corresponding value estimated from the model. Let us first remark that the measured and model estimated values seems to follow the same general tendencies, although they are not really fitting. The fitting error can easily be explained by two factors. First of all it can be the consequence of modeling errors, introduced by non-linearities of the phantom properties such as the natural non-linearity of the liver or some amount of tearing on the surface of the gelatin. It may also be due to the accuracy of the measure of the surface stretch, which is limited by the volume resolution and the fact that a lot of artifacts appear at the tissue surface, deteriorating the quality of the tracking algorithm around this zone.

As expected, we observe that the surface stretch of the model is well regulated to zero when the safety task is set to the highest priority. The measured deformations are also reduced, even if this is less visible from the measures done with the biological tissues.

On the other hand, when the targeting tasks have the priority, more surface stretch tends to be observed. This indicates that the safety task is not always compatible with the targeting tasks, such that its contribution to the control law is sometimes damped by the hierarchical formulation due to the projection on the null space of the targeting tasks (see (4.82)).

It can be noted that the task compatibility only depends on the Jacobian matrices of the different tasks and is independent of their priority levels. Therefore the incompatibility should be observed as well when the priorities are inverted. However the good targeting performances do not seem to be affected by the priority of the safety task. This shows that the safety task is 4.5. CONCLUSION incompatible with only some components of the targeting tasks, corresponding to the lateral translations of the tip. Indeed, when the safety task has the lowest priority, it is simply damped whenever it becomes incompatible with the control of the lateral translations. This results in higher tissue deformation, while the targeting performances are not affected. On the other hand, when the safety task has the highest priority, only the components of the targeting tasks controlling the lateral translations of the needle tip is damped in case of incompatibility. The components corresponding to the tip-based control, i.e. the insertion and the rotation around the needle axis, are indeed always compatible with the safety task since they do not induce much lateral motion of the needle shaft. Hence these components are always available to ensure a certain amount of needle tip steering toward the target, leading to the good targeting performance.

Conclusions:

We have seen than our needle steering framework could be used to accurately reach a target in soft tissues using 3D US as visual feedback. A safety task can also be added using the hierarchical stack of tasks formulation in order to reduce the tissue deformations. Overall, we could see that setting the highest priority to the safety task provides a better control over the deformations of the tissues, while it does not really affect the good targeting performances of the controller. This could be used in clinical applications to perform accurate needle insertions, while also reducing the amount of tissue deformation that is necessary to reach the target.

Conclusion

In this chapter we presented a review of current flexible needle steering methods used to accurately reach a targeted region in soft tissues. We focused on the two main approaches using either lateral motions of the needle base to control the motions of the tip or using only the natural deflection generated by an asymmetric tip during the insertion. Then, we provided an overview of different strategies used to define the trajectory that must be followed by the needle tip.

We proposed a contribution consisting of a steering framework that allows the control of the 6 degrees of freedom of the base of a flexible needle to achieve several tasks during the insertion. Two main tasks were considered: a targeting task to achieve a good steering of the tip toward a target and a safety task to reduce the efforts applied to the needle and the tissues. This framework is also generic enough to integrate both steering approaches using lateral base manipulation and tip-based control in the targeting task.

We then evaluated the performances of the framework through several experiments using a closed-loop visual feedback on the needle provided either by cameras or by the 3D ultrasound modality. These experiments demonstrated the robustness of the control framework to modeling errors and showed that it could achieve a good targeting accuracy even after some motions of the tissues occurred. Several ways to fulfill the safety task were compared and it was found that aligning the needle base with the insertion point during the insertion could provide a trade-off between needle bending and deformations of the tissues.

Overall the framework proved to enable the accurate steering of the tip of the needle toward a target while ensuring low deformations of the tissues. However we only considered virtual targets that were fixed in space and the reduction of the deformations of the tissues was only assessed in stationary tissues. In order to adress these two points, in chapter 5 we will consider the compensation of external motions of the tissues during the needle insertion. The control framework will be extended to cope with moving tissues and to integrate force feedback in the control law.

Chapter 5

Needle insertion with tissue motion compensation

In chapter 4 we proposed a framework to steer a beveled-tip flexible needle under visual guidance. This framework uses all 6 degrees of freedom of the needle base to provide increased targeting performances compared to only using the natural deflection of the beveled-tip. It also allows other tasks to be fulfilled at the same time, such as ensuring the safety of the insertion procedure for the patient. In particular the efforts exerted by the needle on the tissues should be reduced to the strict minimum to avoid further damage caused by the needle. However these efforts can not only be due to the manipulation of the needle but they may also be due to the motions of the tissues themselves.

In this chapter we focus on the compensation of such motions of the tissues during the insertion of a needle. The effect of tissue motions on the performances of the needle tracking has already been covered in chapter 3 and we will focus on tracking the motions of a real target in this chapter. We also propose further adaptations of the steering framework designed in chapter 4 to decrease the risks of tearing the tissues due to the lateral tissue motions. We will consider the case of force feedback to perform the steering with motion compensation.

The chapter is organized as follows. In section 5.1, we first present some possible causes of tissue motions and an overview of current available techniques that may be used for motion compensation. We then propose some extensions of our control framework in section 5.2 to handle motion compensation via visual feedback or force feedback. The tracking of a moving region of interest using the ultrasound (US) modality will be the focus of section 5.3. Finally in section 5.4 we report the results obtained using the proposed framework to perform needle insertions in moving ex-vivo tissues using 2D US together with electromagnetic tracking as position feedback as well as force feedback for motion compensation. The work presented in this chapter was published in an international journal article [CSB + 18].

Tissue motion during needle insertion

Tissue motion is a typical issue that arises during needle insertion procedures. When the procedure is performed under local anesthesia it is possible that the patient moves in an unpredicted manner. In that case, general anesthesia can be needed to reduce unwanted motions [START_REF] Flaishon | An evaluation of general and spinal anesthesia techniques for prostate brachytherapy in a day surgery setting[END_REF]. Whatever the chosen anesthesia method, physiological motions of the patient still occur, mainly due to natural breathing. Motion magnitude greater than 1 cm can be observed in the case of insertions performed near the lungs, like lung or liver biopsies [HMB + 10].

A first consequence of tissue motions is that the targeted region is moving. This can be compensated for by using real-time visual feedback to track the moving target and a closed-loop control scheme to insert the needle toward the measured position of the target. Target tracking using visual feedback is further discussed in the next section 5.3.

Another point of concern in current works on robotic assisted procedures is that the needle is fixedly held by a mechanical robotic system. In the case of base manipulation control, a long part of the needle is outside of the tissues at the early stage of the insertion. Tissue motions can then induce a bending of this part of the needle and modify the orientation of the needle tip. This can greatly influence the resulting tip trajectory, especially if the insertion was planned pre-operatively for an open-loop insertion. In the case of tip-based control, the robotic device is often maintained close to the tissue surface to avoid any bending and buckling of the flexible needle outside the tissues. Hence the needle cannot really bend or move laterally, inducing direct damage to the tissues if the lateral motions of the tissues are large. Motion compensation is thus necessary to limit the risks of tearing the tissues. Many compensation methods exist and have been applied in various cases.

Predictive control: Predictive control can be used to compensate for periodic motions, like breathing. In this case, the motions of the tissues are first estimated using position feedback and then used to predict the future motions such that they can then be compensated for.

Cameras and visual markers can be used to track the surface of the body as was done by Ginhoux et al. [GGdM + 05]. However this does not provide a full information on what is happening inside the body and anatomical imaging modalities can be used instead. For example, Yuen et al. [YPV + 10] used 3D ultrasound (US) for beating heart surgery to track and predict the 1D motions of the mitral annulus in the direction of a linear surgical tool. The main drawback of this kind of predictive control is that the motion is assumed to be periodic with a fix period. This can require placing the patient under artificial breathing, which is usually not the case for classical needle insertions. In the last example, motion compensation of the beating heart was actually performed using a force sensor located between the tissues and the tip of the surgical tool. The motion estimation provided by the visual feedback was only used as a feed-forward to a force controller.

Force feedback: Force control is another method used to perform motion compensation. For needle insertion procedures, a separate force sensor was used by Moreira et al. [START_REF] Moreira | Towards physiological motion compensation for flexible needle interventions[END_REF] to estimate the tissue motions in the insertion direction. The estimated motion was used to apply a periodic velocity to the needle in addition to the velocity used for the insertion.

Impedance or admittance controls are also often used to perform motion compensation since tissue damage can directly be avoided by reducing the force applied to the tissues. This usually requires to first model the dynamic behavior of the tissues. Many models have been proposed for this purpose [START_REF] Moreira | Viscoelastic model based force control for soft tissue interaction and its application in physiological motion compensation[END_REF]. Atashzar et al. [AKS + 13] attached a force sensor directly to a needle holder. The force sensor was maintained in contact with the surface of the tissues during the insertion, allowing the needle holder to follow the motions of the tissues. While axial tissue motions could be accurately compensated for, lateral tissue cutting may still occur in such configuration since the tissues can slip laterally with respect to the sensor. The force sensor can also be directly attached between the manipulator and the needle, as was done by Cho et al.

[CSK + 15][KSKK16]
. This way, lateral tissue motions could be compensated for. Motion compensation in the insertion direction is however difficult to perform in this case. Indeed, the insertion naturally requires a certain amount of force to overcome the friction, stiction or tissue cutting forces, such that it is difficult to separate the effect of tissue motions from the necessary insertion forces.

Since cutting the tissues in the insertion direction is necessary during the needle insertion procedure, we choose to focus only on lateral tissue motions. These lateral motions are also likely to cause more damage due to a tearing of the tissues. In order to be able to adapt to any kind of lateral motions, such as unpredictable patient motions, in the following we do not consider the case of predictive control. Instead we propose to adapt the needle steering framework that we defined in chapter 4, such that it can incorporate force feedback in a reactive control to compensate for tissue motions.

Motion compensation in our task framework

In this section we present an extension of the needle steering framework that we proposed in section 4.3 in order to enable motion compensation. We only consider the case of lateral motion compensation to avoid tissue tearing. Compensation in the insertion direction is less critical in our case since it only has an effect at the tip of the needle, which is already controlled by a task designed in section 4.3.4.1 to reach the target. COMPENSATION Motion compensation can easily be integrated in our needle steering framework by adding a task to the controller. In the following, we first discuss the use of the safety tasks that were designed in section 4.3.4.2 and then we propose a new task design to use the force feedback provided by a force sensor.

Geometric tasks: A lateral motion of the tissues is equivalent to moving the rest position of the path cut by the needle in the tissues (see section 2.4.2 for the definition of this path), which also modifies the initial position of the insertion point. The safety tasks designed previously to provide a safe behavior of the insertion in stationary tissues can thus directly be used to perform motion compensation.

The task designed to minimize the distance between the insertion point and the needle shaft can naturally compensate for the tissue motions since the needle shaft remains close to the insertion point.

The task designed to align the needle base with the insertion point will also naturally follow the tissue motions. In this case it is possible that the needle base only rotates to align with the moving insertion point but does not translate. However, if the needle base does not translate while the tissues are moving laterally then the needle tip deviates from its desired trajectory. In this case, motion compensation can be obtained using the combination of the safety task with a targeting task that controls the lateral motions of the tip, such as the tip translations or the alignment of the tip axis with the target.

The task designed to minimize the bending of the needle will also be sensitive to tissue motions. Indeed, if the needle is in a state of minimal bending energy, then an external motion of the tissues introduces an additional bending of the needle that the task will compensate. However, this task should not be used for stability reasons, as was discussed in section 4.4.1.2.

The main issue concerning the implementation of these safety tasks is that the initial rest position of the insertion point must be known. This rest position can not be observed directly, even with external tissue tracking, since the observable position of the insertion point results from the lateral interaction with the needle. Therefore an estimation is required, for example using the model update method that we proposed in section 3.5.2, such that it gives a correct estimation of the real state of the tissues. However, as was discussed in section 3.6, an exact estimation of the position of the tissues is difficult to obtain due to their non-linear properties and the modeling approximations.

Therefore, we propose instead to use force feedback, which directly provides a measure of the interaction of the needle with the tissues and does not rely on a good estimation of the tissue position.

Force feedback: The ultimate goal of the motion compensation is to reduce the lateral efforts exerted by the needle on the tissues in order to avoid the tearing of the tissues. A task can then directly be designed to minimize these efforts.

In practice it is hard to measure directly the forces applied on the tissues at each point of the needle, as it would require a complex design to place force sensors all along the needle shaft. The forces could be retrieved indirectly by using the full shape of the needle and its mechanical properties. This would require integrated shape sensors in the needle, like fiber Bragg grating (FBG) sensors [PED + 10], which is not always desirable due to the additional design complexity. An imaging modality allowing a full view of the needle could also be used. However, viewing the whole needle is not possible with ultrasound (US) imaging since it is limited to the inside of the tissues. It could be possible to use 3D computerized tomography (CT) or magnetic resonance imaging (MRI), however their acquisition time is too slow to be used for real-time control.

The ideal case would be to measure the forces at only one location of the needle, so that it is not required to use special modifications of the needle itself. It can be noted that in a static configuration the total force exerted at the base of the needle corresponds to the sum of the forces exerted along the needle shaft. Inertial effects can usually be ignored in practice because of the low mass of the needle, such that the static approximation is valid in most cases. Therefore minimizing the lateral force exerted at the base of the needle should also reduce the efforts exerted on the tissues. Therefore in the following we propose to design a task for our steering framework in order to minimize this lateral force.

Lateral force reduction task: Let us define the lateral component f l ∈ R 2 of the force exerted on the needle base. As mentioned previously, we ignore the axial component since it is necessary for the insertion of the needle. The task Jacobian J f ∈ R 2×6 and the desired variations ḟ l,d of the task are defined such that

ḟ l = J f v b , (5.1) ḟ l,d = -λ f f l , (5.2)
where λ f is a positive control gain that tunes the exponential decrease rate of f l and we recall that v b is the velocity screw vector of the needle base. The task Jacobian J f is computed from the interaction model using the finite difference method (4.27) presented in section 4.3.3. For this computation, the lateral force can directly be computed as the shear force applied at the level of the needle base. Using the two-body model defined in sec-COMPENSATION tion 2.4.2, this can be expressed according to

f l = EI d 3 c N (l) dl 3 l=0 , ( 5.3)
where we recall that E is the Young's modulus of the needle, I is its second moment of area and c N is the spline curve representing the needle. This task will be used in the followings to perform motion compensation when a force sensor is available to provide a measure of the interaction force at the base of the needle. This measure will be used as the input to the control law (5.2).

However, motion compensation during a needle insertion procedure is not limited to the reduction of the damage done to the tissues. In order to obtain good targeting performances while the tissues are moving, the motions of the target should also be measured. Therefore, in the following we focus on the tracking of a moving target using an imaging modality.

Target tracking in ultrasound

In all previous experiments we only considered the case of virtual targets. However, in practice, the needle should be accurately steered toward a real target. The target can be moving, either due to physiological motions of the patient or due to the effect of the insertion of the needle on the tissues. In this section we present a tracking algorithm that we developed to follow the motion of a moving spherical target in 2D ultrasound images.

Target tracking in 2D ultrasound

We use a custom tracking algorithm based on the Star algorithm [START_REF] Friedland | Automatic ventricular cavity boundary detection from sequential ultrasound images using simulated annealing[END_REF] to track the center of a circular target in 2D ultrasound (US) images. This kind of tracking has proved to yield good performances for vessel tracking [GSM + 07]. The process of the tracking algorithm is described in Alg. 1 and illustrated in Fig. 5.1 and we detail its functioning in the following.

Template matching: This kind of techniques is widely used in image processing in general and consists in finding a patch of pixels in an image that corresponds the best to another reference patch. Many similarity criteria can be used to assess the resemblance between two patches, like the sum of square differences, the sum of absolute differences or the normalized cross correlation, each having their pros and cons.

The reference patch can also be defined in two main ways. The first one consists in extracting a patch in the previous image at the location of the object. This way the object can be tracked all along the image sequence even if its shape changes. However the accumulation of errors can cause Algorithm 1: Target tracking: initialization is performed manually by selecting the target center p center and radius r in the image as well as the number N of rays for the Star algorithm. A square pixel patch I patch centered around p center is extracted for the template matching.

I patch , p center , r, N ← INITIALIZE_TRACKING(); while Tracking do I ← ACQUIRE_IMAGE(); p center ← TEMPLATE_MATCHING(I, I patch ); E ← ∅; for i ∈ [0, N -1] do θ ← 2πi N ; Ray ← TRACE_RAY(p center , 2r, θ)                    Star algorithm ; p edge ← EDGE_DETECTION(Ray); E ← E ∪ p edge ; end p center , r ← CIRCLE_FITTING(E); I patch ← EXTRACT_REFERENCE_PATCH(I, p center ); end
the tracking to drift. The second one is to take a capture of the object of interest at the beginning of the process and keep it as a reference. This allows avoiding drifts but the tracking can fail if the object shape is changing.

In our case we first apply a template matching between two successive images to get a first estimation of the target motion. This is represented by the TEMPLATE_MATCHING function in Alg. 1. We chose here to take the sum of square differences as similarity measure because it is fast to compute and usually yields good matching. The possible drift will be canceled by the following step of the algorithm, which is the Star algorithm.

Star algorithm: We use the Star algorithm to refine the tracking and to remove the drift obtained with successive template matching by exploiting the a priori shape of the target. The Star algorithm is initialized around the center of the target estimated by the template matching. Angularly equidistant rays are then projected from the target center (see Fig. 5.1). The length of each ray is chosen such that it is higher than the diameter of the target to ensure that each ray is crossing a boundary of the target. An edge detector is run along each ray to find these boundaries. Contrary to the boundaries of a vessel which are almost anechoic, we consider here an hyperechoic target.

Using a classical gradient-based edge detector as was done for the needle tracking in camera images (section 3.4.1), false edge detection could arise due to noise and inhomogeneities inside the target. To reduce this effect, we Figure 5.1: Illustration of the Star algorithm used for the tracking of a circular target in 2D ultrasound. The blue dot is an initial guess of the target center from which rays are projected (blue lines). The estimation of the target center (green cross) is obtained using circle fitting (green circle) on the detected boundaries along each ray (yellow dots). find the boundary along each ray as the point which maximizes the difference between the mean intensities on the ray before and after this point. This is equivalent to finding the distance l edge along each ray such that

l edge = arg min L∈[0,2r] 1 L L 0 I(l)dl - 1 2r -L 2r L I(l)dl (5.4)
where 2r is the length of the ray and I(l) is the pixel intensity along the ray at the distance l from the initial center (see blue dot and lines in Fig. 5.1). Finally a circle fitting is performed on the detected boundaries to find the center of the target as illustrated in Fig. 5.1. This new estimation of the target center is used to extract a new reference path for template matching and the whole process is repeated for the next image.

Both steps of the algorithm are complementary. Template matching can be used to find the target in the whole image if necessary; however its performances are degraded by noise and intensity variations, which cause a drift over time. On the contrary, the Star algorithm can find the real center of the target and adapt to noise, changes of intensity and, up to a certain extend, to changes of the shape of the target. However it requires that the initial guess of the center lies inside the real target in the image. Template matching is thus a good way to provide this first initialization. Overall this tracking algorithm is relatively robust and can be used to track a moving target in 2D US images, in spite of speckle noise or shape variations. It can also easily be adapted to track a 3D spherical target in 3D US volumes.

Target tracking validation in 2D ultrasound

In this section we provide the results of experiments performed to validate the performances of the tracking algorithm that we developed in previous section.

Experimental conditions (setup in the Netherlands): The UR5 robot is used to move a gelatin phantom with embedded play-dough spherical targets. We use the 3D wobbling probe and the ultrasound (US) station from Siemens to acquire the 2D US images. A cross section of the volume is selected to be displayed on the screen of the station such that it contains the target and is normal to the probe axis (US beam propagation direction). The screen of the US scanner is then transferred to the workstation using a frame grabber. The acquisition parameters of the US probe are set to acquire 42 frames during a sweeping motion with an angle of 1.08 • between successive frames. The field of view of each frame is set to 70 • and the acquisition depth is set to 10 cm, resulting in the acquisition of one volume every 110 ms. The targets are between 24 mm and 64 mm from the probe transducer in the experiments, which leads to a maximum resolution of the US image between 0.45 mm × 0.73 mm and 0.70 mm × 1.49 mm.

Experimental scenario: A 3D translational motion is applied to the phantom to mimic the displacement of the liver during breathing [HMB + 10]. The applied motion m(t) has the following profile:

m(t) = a + b cos 4 ( π T t - π 2 ), (5.5)
where a ∈ R 3 is the initial position of the target, b ∈ R 3 is the magnitude of the motion and T is the period of the motion. The magnitude of the motion is set to 7 mm and 15 mm respectively in the horizontal and vertical directions in the image, which corresponds to a typical amplitude of motion of the liver during breathing [HMB + 10]. No motion is set in the out of plane direction. The period of the motion is set to T = 5s. After manual initialization, the tracking is performed for a duration of 30 s corresponding to 6 periods of the motion.

Results:

The position of the tracked target is compared with the groundtruth obtained from the odometry of the UR5 manipulator. An example of the evolution of the target position is shown in The motion described by (5.5) is applied to the gelatin phantom with a period T = 5s. The global mean tracking error is 3.6 mm for this experiment. However it reduces to 0.6 mm after compensating for the delay of about 450 ms introduced by the data acquisition.

the conversion of the volume to Cartesian space, the extraction of the slice to display on the screen, the transfer of the image to the workstation and finally the tracking process. The sweeping takes around 110 ms and the mean tracking time is 300 µs, which indicates that the remaining latency of about 340 ms should mostly be due to the post-scan conversion and the frame grabbing.

In order to assess the quality of the tracking algorithm, the actual positioning accuracy is measured by adding a delay to the ground truth signal. The mean tracking errors over time between the delayed ground-truth and the measures are summarized in Table 5.1. Sub-millimeter accuracy is obtained, which is is sufficient for most medical applications.

We can also observe that the tracking accuracy is lowered by the distance of the target from the probe. This is due to two factors mentioned in section 3.2. First we use a convex wobbling probe, which means that the distance between the different US beams increases as they get further away from the transducer. Additionally each beam also tends to widen during their propagation due to the diffusion phenomenon. Overall the resolution of the 2D image extracted from the 3D volume naturally decreases when its distance from the probe increases. This confirms that the algorithm yields excellent tracking performance and is only limited by the resolution and the latency of the acquisition system. Hence we use this algorithm in the following to perform needle insertion toward real moving targets. Redefinition of control inputs and tasks: Using this setup, the control inputs consist of the velocity screw vector v U R ∈ R 6 of the end-effector of the UR3 plus the 2 velocities v N ID ∈ R 2 of the NID. Hence, we define the control vector v r ∈ R 8 of the whole robotic system as

v r = v U R v N ID .
(5.6)

In the following we assume that v U R is expressed as the velocity screw vector of the frame of the tip of the NID, corresponding to the frame {F b } depicted in Fig. 5.4.

In order to use our steering framework based on task functions with this system, the Jacobian matrices associated to the different tasks that we defined in sections 4.3.4 and 5.2 need to be modified to take into account the additional degrees of freedom (DOF) of the NID. The Jacobian matrix J ∈ R n×8 associated to a task vector e ∈ R n of dimension n is now defined Figure 5.5: Illustration of the three different configurations used to insert the needle. The needle insertion device (NID) is shown in black and the needle is the green line. For configurations 1 and 2, the needle is fully outside and does not slide any further in the NID. For configurations 3, it starts fully inside and can slide in the NID. No constraints is added on the external motion of the NID for configuration 1, while remote center of motion (RCM) is applied around the insertion point for configurations 2 and 3. Additionally, no translations of the tip of the NID is allowed for the third configuration. such that ė = J v r .

(5.7)

Note that we still use our needle model and the method defined in section 4.3.3 to compute the Jacobian matrices. However, the method is adapted to add the two additional DOF of the NID. For simplicity, in the followings we will keep the same notations that we used in sections 4.3.4 and 5.2 for the Jacobian matrices of the different tasks. We will also refer to the equations presented in these sections for the definitions of the tasks.

Insertion configurations and control laws: We compare three different insertion configurations, as depicted in Fig. 5.5.

The first two configurations are used to simulate the case of a needle held by its base. The needle is fully outside of the NID and no control of the translation stage inside the NID is performed, which is equivalent to having a 10.8 cm long needle held by its base. A remote center of motion around the insertion point is added for the configuration 2.

For the third configuration, the tip of the NID (center of frame {F b } in Fig. 5.4) is set in contact with the surface of the phantom and the needle is initially inside the NID. The insertion is then performed using the translation stage of the NID, resulting in a variable length of the part of the needle that is outside the NID. A remote center of motion is also added around the tip of the NID, which is equivalent to the insertion point in this case.

We use several tasks to define the control associated to each configuration and we fuse them using the classical formulation of the task function framework, as defined by (4.6) in section 4.3.1. Four tasks are common to all configurations and are defined as follows.

• The first task controls the insertion velocity v t,z of the needle tip along the needle axis, as defined by (4.36) and (4.37). We set the insertion velocity v tip to 3 mm.s -1 .

• The second task controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal rotation speed ω z,max is set to 180 • .s -1 and the gain λ σ is set to 10 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 18 • .

• The third task controls the alignment angle θ between the needle tip and the target, as defined by (4.38), (4.42) and (4.43). The control gain λ θ is set to 1. Due to the stiffness of the phantom and the high flexibility of the needle, this task can rapidly come close to singularity once the needle is deeply inserted. In order to avoid high control outputs, this task is deactivated once the needle tip has been inserted 2 cm. Given the insertion velocity and the gain set for this task, this gives enough time to globally align the needle with the target such that tip-based control tasks (first and second tasks) are then sufficient to ensure a good targeting.

• The fourth task is used to remove the rotation velocity ω U R ,z of the UR3 around the needle axis. Indeed, we can observe that this rotation has the same effect on the needle as the rotation ω N ID of the needle inside the NID. However, using the UR3 for this rotation would result in unnecessary motions of the whole robotic arm and of the NID, which could pose safety issues for the surroundings.

Therefore we add a task to set ω U R ,z to zero. The Jacobian matrix J ω U R ,z ∈ R 1×8 and the desired value ω U R ,z,d associated to this task are then defined as

J ω U R ,z = [0 0 0 0 0 1 0 0], (5.8) ω U R ,z,d = 0.
(5.9)

For the first two configurations, an additional task is added to remove the translation velocity v N ID of the needle inside the NID. The Jacobian matrix J v N ID ∈ R 1×8 and the desired value v N ID ,d associated to this task are then defined as

J v N ID = [0 0 0 0 0 0 1 0],
(5.10)

v N ID ,d = 0.

(5.11)

The final control vector v r,1 ∈ R 8 for the first configuration is then computed according to

v r,1 =       J vt,z J σ J θ [0 0 0 0 0 1 0 0] [0 0 0 0 0 0 1 0]       +       v t,z,d σd θd 0 0       .
(5.12)

Note that the task space is here of dimension 5 while the input space is of dimension 8, such that all tasks should ideally be fulfilled.

For the second configuration, a task is added to align the needle base with the initial position of the insertion point at the surface of the phantom, such that there is a remote center of motion. This task is defined via the angle γ between the needle base axis and the insertion point, as defined by (4.66), (4.70) and (4.71). The final control vector v r,2 ∈ R 8 for this configuration is then computed according to

v r,2 =         J vt,z J σ J θ J γ [0 0 0 0 0 1 0 0] [0 0 0 0 0 0 1 0]         +         v t,z,d σd θd γd 0 0         . (5.13)
Note that the task space is here of dimension 6 while the input space is of dimension 8, such that all tasks should ideally be fulfilled.

Finally, a remote center of motion is applied at the insertion point for the third configuration. Since the tip of the NID is directly located at the insertion point, this is achieved by adding a task to remove the translation velocity v U R ∈ R 3 . The Jacobian matrix J v U R ∈ R 3×8 and the desired value v U R ,d associated to this task are then defined as

J v U R = [I 3 0 3×5 ],
(5.14)

v U R ,d = 0, (5.15)
where I 3 is the 3 by 3 identity matrix and 0 3×5 is the 3 by 5 null matrix.

MOTION COMPENSATION USING FORCE FEEDBACK

The final control vector v r,3 ∈ R 8 for this configuration is then computed according to

v r,3 =       J vt,z J σ J θ [0 0 0 0 0 1 0 0] [I 3 0 3×5 ]       +       v t,z,d σd θd 0 0       . (5.16)
Note that the task space is here of dimension 7 while the input space is of dimension 8, such that all tasks should ideally be fulfilled.

Experimental scenario: The needle is first placed perpendicular to the surface of the tissues such that its tip barely touches the phantom surface. Then a straight insertion of 8 mm is performed, corresponding to the minimal length of the needle that remains outside the NID when the needle is fully retracted inside it. This way the tip of the NID is just at the level of the phantom surface for the third configuration. This is also done with the two other configurations such that the initial length of needle inside the phantom is the same for every experiment. Four insertions are performed for each configuration. The virtual target is initialized such that it is 7 cm in the insertion direction and 1 cm in a lateral direction with respect to the needle axis. A different lateral direction is used for each experiment, such that the different targets are rotated about 90 • around the needle axis between each experiment. The controller is then started and stopped once the needle tip reaches the depth of the target.

Results: We measure the interaction force exerted at the base of the needle, i.e. in the frame {F b } depicted in Fig 5 .4, during each experiment. The mean value of the absolute lateral force is summarized for each insertion configuration in Fig. 5.6.

We can see that when the NID is near the surface of the tissues, it induces an increase in the amount of force exerted at the needle base compared to the case where the needle base is far from the tissues. This could be expected since the lateral motion of the needle shaft near the needle base is directly applied to the tissues when the whole needle is inserted. In the opposite case the lateral motion can be absorbed by some amount of bending of the needle body, resulting in less force applied to the tissue.

While it is better to reduce the amount of force applied to the tissue, it is also important for the motion compensation to have a good sensitivity of the force measurements to the needle and tissue motions. Using the first two configurations would result in a small and noisy measured force at the base of the needle, due to the damping of the lateral motions of the tissue by the compliance of the needle. Such measures would neither be useful for the model update algorithm nor for the compensation of the tissue motions. These configurations also have the drawback that they require a motion of the whole robot only to insert the needle, which increases the risk of collision with the surroundings. On the contrary this is not required with the third configuration since the insertion can be performed using the internal translation stage of the NID, which would be better in order to avoid collisions in a medical context. Additionally, this last configuration offers a great sensitivity to the tissue motions, since a small displacement is sufficient to induce a significant measure of force at the needle base. This would be beneficial for the model update as well as for the motion compensation. Therefore, in the followings we will choose to insert the needle using the third configuration.

Needle insertion with motion compensation

We present here the results of the experiments performed to test the performances of our framework during an insertion with lateral tissue motions.

Experimental conditions (setup in the Netherlands): The setup used to hold and insert the needle is the same as in section 5.4.1. The ATI force torque sensor is still used to measure the force applied to the base of the needle and the Aurora electromagnetic (EM) tracker is used to measure the position and direction of the tip of the biopsy needle. The UR5 robot is used to apply a known motion to a phantom. Two phantoms are used, one with porcine gelatin and one with a bovine liver embedded in the gelatin. Artificial targets made of play-dough are placed in the gelatin phantom and We use the two-body model presented in section 2.4.2 with polynomial needle segments of order r = 3 to represent the part of the needle that is outside of the NID, from the frame {F b } depicted in Fig. 5.7 to the needle tip. We fix the length of the needle segments to 1 cm, resulting in one segment of 8 mm when the needle is retracted to the maximum inside the NID and 11 segments with the last one measuring 8 mm when the needle is fully outside. We use a rather hard phantom, such that we set the stiffness per unit length of the model to 35000 N.m -2 . The length threshold to add a new segment to the tissue spline is set to L thres = 0.1 mm.

The length and the pose of the base of the needle model are updated using the odometry feedback from the UR3 robot and the NID. The position of the tissue spline in the model is also updated using the force feedback and the EM feedback as input for the update algorithm that we defined in section 3.5.2. The performances of the update algorithm during these experiments have already been described in section 3.6.1. Figure 5.8 summarizes the whole setup and algorithms used for these experiments.

Control: As explained in section 5.4.1, we consider here the input velocity vector v r of the whole robotic system defined by (5.6). We use three targeting tasks, one motion compensation task and two additional tasks for the control of the system and we fuse them using the classical formulation of the task Figure 5.8: Block diagram representing the experimental setup and control framework used to perform needle insertions in a moving phantom. The UR5 robot applies a motion to a phantom. The position of the target is tracked in ultrasound images. Measures from the force torque sensor and electromagnetic (EM) tracker are used to update the needle-tissue interaction model. The model and all measures are used by the task controller to control the UR3 and the needle insertion device in order to steer the needle tip towards the target while compensating for tissue motions.

MOTION COMPENSATION USING FORCE FEEDBACK

function framework, as defined by (4.6) in section 4.3.1.

The different tasks are defined as follows.

• The first task controls the insertion velocity v t,z of the needle tip along the needle axis, as defined by (4.36) and (4.37). We set the insertion velocity v tip to 3 mm.s -1 .

• The second task controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal rotation speed ω z,max is set to 180 • .s -1 and the gain λ σ is set to 10 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 18 • .

• The third task controls the alignment angle θ between the needle tip and the target, as defined by (4.38), (4.42) and (4.43). The control gain λ θ is set to 1. Due to the stiffness of the phantoms and the high flexibility of the needle, this task can rapidly come close to singularity once the needle is deeply inserted. In order to avoid high control outputs, this task is deactivated once the needle tip has been inserted 2 cm. Given the insertion velocity and the gain set for the task, this gives enough time to globally align the needle with the target such that tip-based control tasks (first and second tasks) are then sufficient to ensure good targeting.

• The fourth task is the safety task and it is chosen to reduce the lateral force f l applied to the base of the needle, as defined by (5.1) and (5.2). The control gain λ f is set to 2.5.

• The fifth task is used to remove the rotation velocity ω U R ,z of the UR3 around the needle axis, as was discussed in section 5.4.1. It is defined by (5.8) and (5.9).

• The sixth task is used to remove the translation velocity v U R ,z of the UR3 along the needle axis. For similar reasons as the previous task, we can observe that this translation is redundant with the insertion of the needle by the NID. However, translating the UR3 in this direction could drive the NID into the tissues, which should be avoided for safety reasons.

Therefore we add a task to set v U R ,z to zero. The Jacobian matrix J v U R ,z ∈ R 1×8 and the desired value v U R ,z,d associated to this task are then defined as

J v U R ,z = [0 0 1 0 0 0 0 0], (5.17)
v U R ,z,d = 0.

(5.18) COMPENSATION

The final control vector v r at the beginning of the insertion is then computed according to

v r =         J vt,z J σ J θ J f [0 0 0 0 0 1 0 0] [0 0 1 0 0 0 0 0]         +         v t,z,d σd θd ḟ l,d 0 0         . (5.19)
Once the needle tip reaches 2 cm under the initial tissue surface, v r is then computed according to

v r =       J vt,z J σ J f [0 0 0 0 0 1 0 0] [0 0 1 0 0 0 0 0]       +       v t,z,d σd ḟ l,d 0 0       .
(5.20)

The inputs of the targeting tasks are computed using the target position measured from the tracking in US images and the pose of the needle tip measured from the EM tracker. The input of the safety task is computed from the lateral force applied at the needle base that is measured from the force sensor. Note that the task space is here of dimension 6 or 7 while the input space is of dimension 8, such that all tasks should ideally be fulfilled.

Registration:

The position of the EM tracking system is registered in the frame of the UR3 robot before the experiments using the method that was presented in section 3.6.1. The force torque sensor is also calibrated beforehand to remove the sensor biases and the effect of the weight of the NID in order to reconstruct the interaction forces applied to the base of the needle. The details of the sensor calibration and the force reconstruction can be found in Appendix A.

In order to accurately reach the target, its position must be known in the frame of the needle tip, such that the inputs of the targeting tasks can be computed. An initial registration step is thus required to find the correspondence between a pixel in a 2D US image and its real location in a common frame with the EM tracker, which is the UR3 robot frame in our case.

The pose of the 3D US probe is first registered beforehand in the UR3 robot frame using the following method. The needle is inserted at different locations near the surface of the phantom and two sets of the positions of the needle tip are recorded, one using the needle manipulator odometry and needle model, and the other one using a manual segmentation of the needle tip in acquired 3D US volumes. Point cloud matching between the two sets is then used to find the pose of the US probe in the frame of the UR3 robot.

The drawback of this method is that it is not clinically relevant, since many insertions are required before starting the real insertion procedure. However, for a clinical integration the pose of the probe could be measured using an external positioning system, such as an EM tracker fixed on the probe or through the tracking of visual markers with an optical localization system.

Before the beginning of each needle insertion, the acquisition of 3D US volumes is launched and a plane normal to the probe axis is selected to be displayed on the US station screen. This plane is chosen such that it contains the desired target for the insertion. The position of the image in space is then calculated from the probe pose and the distance between the probe and the image, available using the US station. Each image can finally be scaled to real Cartesian space by using the size of a pixel, which is also known using the measurement tool available in the US station.

Experimental scenario: Five insertions are performed in each phantom. At the beginning of each experiment the image plane displayed on the US station is selected to contain the desired target. The tracking algorithm is manually initialized by selecting the center of the target and its diameter in the image. The needle is initially inside the NID. An initialization of the insertion is performed by moving the tip of the NID to the surface of the tissues, such that the 8 mm of the needle that remain outside the NID are inserted into the tissues.

The update algorithm, needle insertion and tissue motions are then started and are stopped once the needle tip has reached the depth of the target. The motion applied to the phantom is similar to the motion used for target tracking validation defined by (5.5) in section 5.3.2. The amplitude of the motion is set to 15 mm and 7 mm in the x and z direction of the world frame {F w } depicted in Fig. 5.7, respectively. Several values of the period of the motion are used, between 10 s and 20 s, as recapped in Table 5.2.

Results: An example of the position of the needle tip measured with the EM tracker and the position of the tracked target during an insertion in the liver phantom can be seen in Fig. 5.9. We can see in Fig. 5.9b that both the needle and the target follow the motions of the tissues shown in Fig. 5.9a. The needle tip is steered toward the target and reaches it at the end of the insertion, as can be seen in Fig. 5 Table 5.2: Summary of the conditions and results of the insertions performed in a gelatin phantom and a bovine liver embedded in gelatin. Different periods T are used for the motion of the phantom. The target location in the initial tip frame is indicated for each experiment. The error is calculated as the absolute lateral distance between the needle tip axis and the center of the target at the end of the insertion. The mean and standard deviation of the error for each kind of phantom are presented separately.

Target ). An example of a slice extracted from a final volume in gelatin is depicted in Fig. 5.12, showing that the center of the target can be accurately reached. Table 5.2 gives a recap of the initial position of the targets in the initial frame of the needle tip and the absolute lateral errors obtained at the end of each experiment. We can see that the target can be reached with an accuracy under 4 mm in all cases. This can be sufficient in clinical applications to reach medium sized tumors near moving structures. We can note that we choose to not compensate for the latency introduced by the acquisition system (see section 5.3.1). Indeed, this latency will mostly be unknown during a real operation and can also vary depending on the chosen acquisition parameters, such as the field of view and the resolution of the scanning. This point could be addressed if higher accuracy is required for a specific application.

The mean targeting error is also higher in biological tissues than in gelatin. Several factors may explain this observation. The main reason is certainly that the tracking of the target is more challenging in this case. The target is less visible and the level of noise is increased in the image due to the texture of the tissues, as can be seen when comparing Fig. 5.10 and 5.11. This also limits the accuracy of the detection of the target center in the final 3D US volume. The evaluation of the final targeting accuracy is thus subject to more variability. The inhomogeneity of the tissues can also be a cause of deviation of the needle tip from its expected trajectory. However the tip-based targeting task orienting the bevel edge toward the target should alleviate this effect.

Motion compensation: Finally, let us consider the motion compensation aspect during the insertion. Due to the proximity of the NID with the surface of the phantom, the lateral forces measured at the needle base are very similar to the forces applied to the tissues, such that we can use these measures to estimate the stress applied to the tissues. Table 5.3 gives a recap of the maximum and mean lateral forces applied to the needle base during the insertion. We can observed that these forces are maintained under 1.0 N during all the experiments, which is sufficiently low to avoid significant tearing of the tissues. In clinical practice, this is also lower than the typical forces that can be necessary to puncture the tissues and insert the needle. The framework is thus well adapted to perform motion compensation and to avoid tissue damage. Note that we did not perform full insertions without the motion compensation to compare the forces obtained in this case. However, we could observe during preliminary experiments that applying a lateral motion to the phantom while the needle is inserted in the tissues and fixed by the NID results in a large cut in the gelatin and also damages the needle. A small amount of tearing could still be observed at the surface of the gelatin when the motion compensation was performed, essentially due to the natural weakness of the gelatin and the cutting effect when the needle is inserted while applying a lateral force. Nevertheless, it can be expected that real biological tissues are more resistant and would not be damaged in this case.

Figure 5.13 shows a representative example of the motions of the tissues and the lateral forces measured during an insertion in the bovine liver. Two phases can clearly be distinguished on the force profile. During the first 6 seconds, some fast variations of the force can be observed. This corresponds to the phase where all tasks are active using (5.19). The robotic system is thus controlled to explicitly align the needle tip axis with the target while reducing the applied force. Since the target is initially misaligned, a lateral rotation is necessary, which naturally introduces an interaction with the phantom. Fast motion of the system are thus observed, resulting from the interaction between the alignment and motion compensation tasks. After the tip alignment task has been deactivated, all remaining tasks in (5.20) are relatively independent, since inserting and rotating the needle does not introduce significant lateral forces. This results in a globally smoother motion, where the needle is simply inserted while the lateral motion of the NID naturally follows the motion of the phantom. Therefore motion compensation is clearly achieved in this case. We can see that the lateral force is well driven towards zero when the tissues are moving slowly. However, the amount of lateral force also tends to increase when the velocity of the tissues increases, which is due to the proportional nature of the control law (5.2). This could be improved by modifying the control law in order to reduce the motion following error, for example by adding some integral term in addition to the proportional gain.

Conclusions: These experiments confirm that, when the needle has a great level of flexibility, lateral motion of the needle should only be performed at the beginning of the insertion to align the needle with the target. It allows a fast and efficient way to modify the needle trajectory without having to insert the needle, which could not be possible by exploiting only the natural deflection of the tip. However once the needle is inserted deeper in the tissues, the motion of the base has only a low effect on the tip trajectory compared to its effect on the force applied to the surface of the tissues. Alternating between several tasks depending on the state of the insertion has thus proved to be a good way to exploit the different advantages of each task while reducing their undesirable effects. These experiments also demonstrate that motion compensation can be performed at the same time as the accurate steering of the needle tip toward a target. Here we use only a first order control law for the force reduction task, which proves to be sufficient to yield good motion compensation. A more advanced impedance control could be used to obtain even better results and to reduce even further the applied lateral force [START_REF] Moreira | Viscoelastic model based force control for soft tissue interaction and its application in physiological motion compensation[END_REF].

Overall this set of experiments also demonstrates the great flexibility of our control framework. It shows that the framework can be used in a new configuration, where the needle is not held by its base but is instead inserted progressively in the tissues using a dedicated device. It has also proven to be able to maintain the same general formulation and adapt to 2D US, EM sensing and force feedback.

Conclusion

In this chapter we provided an overview of motion compensation techniques that can specifically be used to follow the motions of soft tissues. We then showed that the framework we proposed in chapter 4 could be easily adapted to provide motion compensation capabilities in addition to the needle steering. We then proposed a tracking algorithm that can follow a moving target in 2D ultrasound images. The performances of this algorithm were validated through experiments showing that it could achieve good tracking up to the resolution provided by the image. Finally we demonstrated the great flexibility of our global framework at handling multiple kinds of feedback modalities and robotic systems through experiments performed in a multi-sensor context and using a system dedicated to needle insertion. We showed that it allows fusing the steering of the tip of a needle with the compensation of lateral motions of the tissues. Results obtained during experimental insertions in moving ex-vivo biological tissues demonstrated that performances compatible with the clinical context can be obtained for both tasks performed at the same time, which constitute a great contribution toward safe and accurate robotic needle insertion procedures.

Conclusion

Conclusions

In this thesis we covered several aspects of robotic needle insertion under visual guidance. In Chapter 1 we first presented the clinical and scientific context of this work and the challenges associated to this context. In Chapter 2 we provided a review on the modeling of the interaction between a needle and soft tissues. We then proposed two 3D models of the insertion of a beveled tip needle in soft tissues and compared their performances. In Chapter 3 we addressed the issue of needle localization in 3D ultrasound (US) volumes. We first provided an introduction to US imaging and an overview of needle tracking algorithms in 2D and 3D US images. We proposed a 3D tracking algorithm that takes into account the natural artifacts that can be observed around the needle location. We used our needle model to improve the robustness of the needle tracking and proposed a method to update the model and take tissue motions into account. In Chapter 4 we first presented a review of techniques used to control the trajectory of a needle during its insertion and to define the trajectory that must be followed by the needle tip. We then proposed a needle steering framework that is based on the task function framework used to perform visual servoing. The framework allows a great flexibility and can be adapted to different steering strategies to control several kinds of needles with symmetric or asymmetric tips. We then validated our framework through several experimental scenarios using the visual guidance provided by cameras or a 3D US probe. In Chapter 5 we considered the case of patient motions during the needle insertion and provided an overview of methods that can be used to compensate for these motions. We then extended our steering framework to handle motion compensation using force feedback. We finally demonstrated the flexibility of our framework by performing needle steering in moving soft tissues using a dedicated needle insertion robot in a multi-sensor context.

In the following we draw some conclusions concerning needle modeling, ultrasound visual feedback, needle steering under ultrasound guidance and compensation of tissue motions during needle insertion. Finally we present some perspectives for future developments of our work.

Needle modeling

We first focused on the modeling of the behavior of a flexible needle being inserted in soft tissues. Fast and accurate modeling of the interaction phenomena occurring during the insertion of a needle is a necessity for the control of a robotic system aiming at the assistance of the insertion procedure. We have seen that models based on kinematics are efficient and allow fast and reactive control. However their efficiency often comes at the cost of a limitation of the phenomena that they can take into account. On the contrary, almost every aspect of an insertion can be described using finite element modeling, from the deformations of the needle to the complex modifications that it can create on the tissues. Nevertheless this complexity can only be obtained through heavy parameterization that requires accurate knowledge of boundary conditions and time consuming computations. A trade-off should thus be found to take into account the main interaction phenomena and to keep a low level of complexity to stay efficient. Mechanics-based models offer such compromise by reducing their computation requirement while still being a realistic representation of the reality. In this thesis we proposed a 3D flexible needle model that is simple enough to yield real-time performances, while still being able to take into account the deformations of the needle body due to its interaction with the moving tissues at its tip and all along its shaft.

Ultrasound visual feedback

A second requirement for the development of a robotic system usable in clinical practice is its ability to monitor the state of the environment on which it is acting. For needle procedure assistance this means knowing the state of the needle and of the tissues. Visual feedback is a great way to deal with this issue by mean of the dense information it can provide on the environment. Additional conditions should also be fulfilled on the nature of the provided images. A great quality of the image is only useful if it can be obtained frequently. On the contrary, images provided at a fast rate can only be used if they contain exploitable information. To this end, the ultrasound (US) modality is one of the best choice since it can provide real-time 2D or 3D relevant data on a needle and its surrounding tissues. Extracting this information in a reliable way is however a great challenge due to the inherent properties of this modality. We have shown a review of the different artifacts that can appear in US images as well as current techniques used to localize a needle in these images. In order to be used in the context of a closedloop control of a robotic system, needle detection algorithms should be fast and accurate. We contributed to this field by proposing a needle tracking method that directly exploits the artifacts created by the needle to find the location of its whole body in 3D US volumes. Ensuring the good consistency of the tracking between successive volumes can be achieved by modeling the expected behavior of the needle. The modeling of the current state of the needle interaction with the tissues can also be improved by exploiting the measures available on the needle. Therefore we fused our contributions by proposing a method to update our interaction model from the measures provided by several sensors and to use the model to improve the quality of the needle tracking.

Needle steering under ultrasound guidance

Once a good model of the insertion process is available along with a reliable measure of the needle location, the closed-loop robotic control of the insertion can be performed. We first reviewed the current techniques available to steer a needle in soft tissues and the different strategies used to define the trajectory of the tip. In order to remain as close as possible from a possible clinical application, we chose to focus on the steering of standard needles and did not consider the case of special designs. Two main methods are usually used in the literature to steer the needle, either manipulating the needle by its base, like usually done by the clinicians, or by exploiting an asymmetry of the tip to deflect the needle from a straight path during the insertion. In order to stay generic, we designed a control framework to manipulate the needle by its base and also to exploit the asymmetry of the tip. This framework also directly uses our previous contribution on needle modeling by using the model in real-time to compute the motions to apply to the base of the needle allowing specific motions of the needle tip. We performed several validation experiments to assess the performances of our framework in needle insertions performed under visual feedback. In particular we showed that the approach is robust to modeling errors and can adapt to tissue or target motions.

Tissue motion compensation during needle insertion

We addressed the topic of the compensation of the patient motions during a needle insertion procedure. The body of a patient may be moving during a medical procedure for several reasons, the most common ones being physiological motions which can hardly be avoided. Tissue motions can have several impacts on the results of a needle insertion procedure. The first one is that the targeted region inside the body is moving. This point should be taken into account by tracking the target and adapt the control of the needle trajectory accordingly. The steering framework that we proposed was already capable of handling this first issue and we complemented it with a target tracking algorithm in ultrasound images. The second important issue concerns the safety of the insertion, which can be compromised if the robotic system is not designed to follow the motions of the patient. In order to address this issue, we proposed an adaptation of our steering framework to be able to perform motion compensation using either visual feedback or force feedback. We demonstrated the performances of our method by performing the insertion of a flexible needle in ex-vivo moving biological tissues, while compensating for the tissue motions. Overall, the results of our experiments confirmed that our global steering framework can be adapted to several kinds of robotic systems and can also integrate the feedback provided by several kinds of sensing modalities in addition to the visual feedback, such as force measurements or needle tip pose provided by an electromagnetic tracker.

Perspectives

We discuss here several extensions and further developments that could be made to complement the current work in terms of technical improvements and clinical acceptance. Both aspects are linked since driving this work toward the operating room requires to first identify the specific needs and constraints of the medical staff, and then translate them into theoretical and technical constraints. In the following we address the limitations that were already mentioned in this manuscript as well as new challenges arising from specific application cases.

Needle tracking

Speed of needle tracking: We presented a review of needle tracking techniques in ultrasound (US) images and volumes. In order to be usable, automatic tracking of the needle should be fast, such that it gives a good measure of the current state of the insertion. Hence efficient tracking algorithms are a first necessity to achieve this goal, which is why we proposed a fast tracking algorithm. However, independently of the chosen tracking algorithm, current works are mostly using 3D volumes reconstructed in Cartesian space. Since the reconstruction of a post-scan volume from pre-scan acquired data requires some time, it introduces a delay in the acquisition. This is a first obstacle for a fast control of the needle trajectory since the needle has to be inserted slowly to ensure that the visual feedback is not completely out of date once available. The time of conversion can be reduced using specific hardware optimization, however a simple solution would be to directly use the pre-scan data to track the needle. While conversion of the whole post-scan volume remains desirable to provide an easy visualization for the clinician, it is not necessary for a tracking algorithm. The needle could be tracked in the pre-scan space and then converted in Cartesian space if needed. Taking a step further, it can be noted that acquiring the pre-scan volume also takes time. Tracking of the needle could be done frame by frame during the wobbling process to provide 2D feedback on the needle section directly once available.

Reliability of needle tracking: In order to be usable for a control application in a clinical context, the tracking of the needle should be reliable. Reliability in the case of tracking in 3D US is challenging because of the overall low quality and the artifacts present in the image, which is the reason why we proposed a method to account for needle artifacts. In general, even for the human eye, it can be difficult to find the location of the needle in a given US volume when other bright linear structures are present. Temporal filtering is usually applied to reduce the size of the region in which to search the needle. In our case we used a mechanical model of the needle to predict the position of the needle in the next volume and we updated the model to take into account tissue motions. However large tissue motions or an external motion of the US probe can cause a large apparent motion of the needle in the volume which is not taken into account by the temporal filtering, resulting in a failure of the tracking. Following the motion of the other structures around the needle could be a solution to ensure the spacial consistency of the tracked needle position. Tracking the whole tissues could be a solution, for example by using methods based on optical flow [TSJ + 13]. Deep learning techniques could also be explored since they show ever improving promising results for the analysis of medical images [LKB + 17].

Active tracking: An accurate tracking of the needle location in the US volume is very important for the good proceedings of the operation. Servoing the position of the US probe could be a good addition to increase the image quality and ease the tracking process. A global optimization of the US quality could be performed [START_REF] Chatelain | Confidence-driven control of an ultrasound probe: Target-specific acoustic window optimization[END_REF]. A control scheme could also be designed to take into account the needle specific US artifacts, such as intensity dropout due to the incidence angle, and to optimize the quality of the image specifically around the needle [START_REF] Chatelain | Real-time needle detection and tracking using a visually servoed 3d ultrasound probe[END_REF].

Needle steering

Framework improvement: The needle insertion framework based on task functions that we proposed can be extended in many ways. First we did not directly consider the case of obstacle avoidance. This could be easily added by the design of a specific avoidance task or by using trajectory planning to take into account sensitive regions [XDA + 09]. A higher level control over the tasks priority should also be added to adapt the set of tasks to the many different situations that can be encountered [START_REF] Mansard | Task sequencing for highlevel sensor-based control[END_REF]. For example targeting tasks could be deactivated in case a failure of the needle tracking has been detected. A specific task could be designed to take the priority in this case and to move the needle in such a way that it can easily be found by the tracking algorithm.

Active model learning: In our control framework we used a mechanicsbased model of the needle to estimate to local effect of the control inputs on the needle shape. We proposed a method to update the local model of the tissues according to the available measures. A first improvement would be to explore the update of other parameters as well, like the tissue stiffness. However it is possible that the model does not always accurately reflect the real interaction between the needle and tissues, mainly due to the complex nature of biological tissues. A model-less online learning of the interaction could be explored, using the correlation between the control motions applied to the needle and its real measured effect on the needle and tissue motions. The steering strategy could also be modified to optimize this learning process, for example by stopping the insertion and slightly moving the needle base in all direction to observe the resulting motions of the needle tip and target.

Clinical integration

Before integrating the proposed framework into real clinical workflow, many points needs to be taken into account and discussed directly with the clinical staff.

System registration: A first requirement for a good integration into the clinical workflow is that the system should be easy to use out-of-the-box, without requiring time consuming registration before each operation. In the case where the insertion is done using 3D ultrasound (US) to detect both the needle and the target in a same frame, we have proposed a simple registration method to estimate the pose of the US probe in the frame of the needle manipulator. The method only requires two clicks of the operator through a GUI and is necessary anyway to initialize the tracking of the needle. We showed that this was sufficient to achieve good targeting performances thanks to the robustness of the control method, however the estimation of the motions of the tissues proved to be more dependent on an accurate registration. An online estimation of the probe pose could be used to refine the initial registration. This may require additional sensors, such as fiber Bragg grating sensors integrated into the needle [PED + 10], to be able to differentiate between the motion that is due to the tissues or the probe. In clinical practice the US probe is also unlikely to stay immobile during the whole procedure. This can for example be because it is manually held by the clinician or because the field of view of the probe is too narrow and it has to be moved to follow the needle and the target. Online estimation would also be an advantage in those cases, and sensors could provide a direct feedback on the probe pose, like electromagnetic trackers or external cameras. A specific mechanical design could also be used to mechanically link the probe to the needle manipulator [YPZ + 07]. In this case the probe pose is known by design and a registration step is unnecessary.

Tele-operation: The method we have proposed so far was aimed at performing a fully automated needle insertion. This nowadays still remains a great factor of rejection among the medical community. However the clinician can easily be integrated into the framework. A first possibility is to consider the robot as an assistant which can perform some predefined automated tasks, such as a standby mode, during which only tissue motion compensation is performed, and an insertion mode, during which the needle is automatically driven toward the target. The clinician would only have to select the set of tasks currently being performed. This way the global flow of the operation would still be controlled by a human operator while the low level complex fusion of the tasks would be handled by the system. However this only leaves a partial control to the clinician on the real insertion procedure. A second possibility is to give a full control to the clinician over one of the tasks and let the others to the system. For example the clinician can control the trajectory of the needle tip while the robot transparently handles the orientation of the bevel and motion compensation. A haptic interface could be used to provide a guidance on the optimal trajectory to follow to avoid some predefined obstacles and reach the target. Other kinds of haptic feedback could also be explored, as for example a feedback on the state of the automatic tasks performed by the system or the compatibility of the clinician's task with the other tasks. A visual feedback could also be provided to the clinician such that the control of the tip trajectory could be defined directly in the frame of a screen instead of the frame of the real needle.

Clinical validation:

We have shown that our steering framework could be adapted on several robotic systems. In order to go toward clinical integration, repeatability studies have to be conducted in biological tissues to assess the robustness of the method with a specific set of hardware components. These studies should be repeated for each envisioned set of hardware and the performances should be evaluated in accordance with the exact application that is considered. Performance requirements can indeed be different for each application, as for example in moving lung biopsies and prostate brachytherapy.

Long-term vision: Finally, one can believe that fully autonomous surgeon robots will one day become reality. Contrary to a human surgeon, robotic systems are not limited to two hands and two eyes. They can have several dexterous arms that perform manipulations with more accuracy than a human. They can also integrate many feedback modalities at once, allowing a good perception of many different aspects of their environment. This is currently not enough to provide them with a good understanding of what is truly happening in front of them and the best action that should be performed. However, with the ever improving performances of artificial intelligence, it may be possible in the future that robotic systems have a better comprehension and adaptability to their environment. They could then be able to chose and perform with great efficiency the adequate task, as for example a medical act, that is the best adapted to a current situation taken in its globality. Before reaching this state, systems and techniques should first be developed that can autonomously perform narrow tasks with the best efficiency, such as a needle insertion. These could then be connected together to form a generic expert system. Dans cette thèse nous nous concentrons sur le contrôle automatique de la trajectoire d'une aiguille flexible à pointe biseautée en utilisant la modalité échographique comme retour visuel. Nous proposons un modèle 3D de l'interaction entre l'aiguille et les tissus ainsi qu'une méthode de suivi de l'aiguille dans une séquence de volumes échographiques 3D qui exploite les artefacts visibles autour de l'aiguille. Ces deux éléments sont combinés afin d'obtenir de bonnes performances de suivi et de modélisation de l'aiguille même lorsque des mouvements des tissus sont observés. Nous développons également une approche de contrôle par asservissement visuel pouvant être adaptée au guidage de differents types d'outils longilignes. Cette approche permet d'obtenir un contrôle précis de la trajectoire de l'aiguille vers une cible tout en s'adaptant aux mouvements physiologiques du patient. Les résultats de nombreux scénarios expérimentaux sont présentés et démontrent les performances des différentes méthodes proposées.

Abstract

The robotic guidance of a needle has been the subject of a lot of research works these past years to provide an assistance to clinicians during medical needle insertion procedures. However, the accurate and robust control of a needle insertion robotic system remains a great challenge due to the complex interaction between a flexible needle and soft tissues as well as the difficulty to localize the needle in medical images.

In this thesis we focus on the ultrasound-guided robotic control of the trajectory of a flexible needle with a beveled-tip. We propose a 3D model of the interaction between the needle and the tissues as well as a needle tracking method in a sequence of 3D ultrasound volumes that uses the artifacts appearing around the needle. Both are combined in order to obtain good performances for the tracking and the modeling of the needle even when motions of the tissues can be observed. We also develop a control framework based on visual servoing which can be adapted to the steering of several kinds of needle-shaped tools. This framework allows an accurate placement of the needle tip and the compensation of the physiological motions of the patient. Experimental results are provided and demonstrate the performances of the different methods that we propose.

Figure 1 . 1 :

 11 Figure 1.1: Example of (a) continuum robot (taken from [CMC + 08]) and (b) concentric tubes (taken from [WRC09]).

Figure 1 . 2 :

 12 Figure 1.2: Illustration of several kinds of needle tip.

  Fig. 1.3. They are usually designed for a specific kind of intervention, such as prostate interventions under ultrasound (US) imaging [YPZ + 07] [HBLT12]. Special robots have also been designed to be compatible with the limitations imposed by computerized tomography (CT) scanners [MGB + 04], magnetic resonance imaging (MRI) scanners [MvdSK + 17] or both [ZBF + 08].

Figure 1 . 4 :

 14 Figure 1.4: Example of special designs of the needle tip: (a) multi-segment needle (taken from [KFRyB11]), (b) one degree of freedom active pre-bent tip (taken from [AGL + 16]) and (c) two degrees of freedom active prebent tip (taken from [RvdBvdDM15]).

  Figure 1.5: Pictures of the robotic systems used for the experiments: (a) system used in France and (b) system used in the Netherlands.

Figure 1 . 6 :Figure 1

 161 Figure 1.6: Picture of the stereo camera system and one gelatin phantom used for the experiments.

( a )

 a Figure 1.8: Picture of the needle used for the experiments in France.

Figure 2 . 1 :

 21 Figure 2.1: Illustration of the reaction forces applied to the needle tip by the tissues depending on the tip geometry. A symmetric tip, on the right, induces symmetric reaction forces. An asymmetric tip, on the left, induces asymmetric reaction forces which can modify the tip trajectory.

Figure 2. 3 :

 3 Figure 2.3: Illustration of finite element modeling (taken from (a) [ODGRyB13] and (b) [CAR + 09]).

  Figure 2.4: Illustration of mechanics-based models of needle-tissue interaction using either virtual springs or a continuous load.

Figure 2

 2 Figure 2.6: Illustration of the reaction forces applied on each side of the bevel (depicted on the right). Point O corresponds to the end of the 3D curve representing the needle. The current velocity of the needle tip v t defines the angle β in which the cutting occurs in the tissues.

Figure 2

 2 Figure 2.8: Picture of the setup used to acquire the trajectory of the tip of a real needle inserted in a gelatin phantom. The frame attached to the needle base is denoted by {F b }.

  {F b }, depicted in Fig. 2.8, and is one of the following • No motion (straight insertion) • Translation of 2 mm or -2 mm along x axis • Translation of 2 mm or -2 mm along y axis • Rotation of 3 • or -3 • around x axis • Rotation of 3 • or -3 • around y axis • Rotation of 90 • , -90 • or 180 • around z axis

CHAPTER 2 .Figure 2 Figure 2 . 10 :

 22210 Figure 2.9: Tip position obtained when a translation is applied along the x axis of the base frame between two insertion steps along the z axis. Measures are shown with solid lines, virtual springs model with long-dashed lines and two-body model with short-dashed lines.

2. 5 .

 5 VALIDATION OF THE PROPOSED MODELS Rotation around x axis in the needle base frame {F b }

Figure 2 . 11 :

 211 Figure 2.11: Tip position obtained when a rotation is applied around the x axis of the base frame between two insertion steps along the z axis. Measures are shown with solid lines, virtual springs model with long-dashed lines and two-body model with short-dashed lines.

Rotation around z

  axis in the needle base frame {F b }

FinalFigure 2 . 14 :

 214 Figure 2.14: Absolute final error between the position of the simulated needle tip and the measured position of the real needle tip. For each type of base motions, the mean and standard deviation are calculated using the final position error across 3 different insertions performed in the phantom.

Figure 2 . 15 :

 215 Figure 2.15: Average absolute error during the whole insertion between the position of the simulated needle tip and the measured position of the real needle tip. For each type of base motions, the mean and standard deviation are calculated over the whole length of the insertion and across 3 different insertions performed in the phantom.

Figure 2 . 16 :

 216 Figure 2.16: Computation time needed to get the shape of the needle from the base pose and position of the tissue model (virtual springs or spline segments).

Figure 3 . 1 :

 31 Figure 3.1: Illustration of the reconstruction of an ultrasound (US) scan line from the delay of propagation of the US wave. A first interface is encountered after a time t1 and a second one after time t 2 > t 1 . A part of the US wave reaches back the transducer after a total time of 2t 1 and a second part after 2t 2 . The distance of the first and second interfaces from the transducer are then computed as ct 1 and ct 2 , respectively.

Figure 3 . 2 :

 32 Figure 3.2: Illustration of the configuration of the piezoelectric elements (red) on linear and convex ultrasound transducers.

Figure 3 . 3 :

 33 Figure 3.3: Illustration of the effect of US beam width on the lateral and out of plane resolution of an US probe. Piezoelectric elements are represented in red and only a linear transducer is depicted here.

. 8 )Figure 3 . 4 :

 834 Figure 3.4: Illustration of 2D post-scan conversion for linear (top) and convex (bottom) transducers.

Figure 3 . 5 :

 35 Figure 3.5: Illustration of 3D post-scan conversion for a convex transducer wobbling probe.

Figure 3

 3 Figure 3.6: Illustration of several phenomena leading to the appearance of needle artifacts in ultrasound images: needle reverberation artifact, side lobes (blue arrows) artifact and reflection out of the transducer.

Figure 3 . 7 :

 37 Figure 3.7: Two orthogonal cross sections of a 3D ultrasound (US) volume showing the artifacts present around a needle. The needle is in the plane of the picture on the left and the right picture shows a cross section of the needle. In both images the US wave is coming from the left.

  Figure3.9: Illustration of the gradient-based algorithm used to track the needle in the images acquired by two stereo cameras. Steps of the algorithm: 1) initialization of control points from the previous needle location, 2) detection of the minimum and maximum gradient in normal directions, 3) update of the control points and polynomial fitting, 4) tip detection using gradient along the needle shaft direction.

  Figure 3.10: Illustration of the sub-cost functions used for the local tracking algorithm. The voxel intensities in the dark blue box should be low, so they are subtracted from sub-cost J 1 (see eq.(3.39)), while the ones in the orange box should be high and are added to J 1 . Similarly, voxels in the green boxes are added to the sub-cost J 2 (see eq.(3.41)). Once the needle has been tracked laterally by maximizing the total cost function J (see eq.(3.38)), a research of the tip is performed along the tangent at the extremity of the needle to maximize the function J 4 . The voxel intensities in the light blue box are subtracted from J 4 (see eq.(3.44)) while the ones in the yellow box are added.

Figure 3 . 11 :

 311 Figure 3.11: Picture of the setup used to acquire volume sequences of needle insertions in a gelatin phantom. The Viper s650 robot holds the needle on the left and the Viper s850 robot holds the probe on the right.

Figure 3 . 14 :

 314 Figure 3.14: Illustration of the principle of Bayesian filtering

Figure 3 . 16 :

 316 Figure 3.16: Illustration of the unscented Kalman filter

Figure 3 .

 3 Figure 3.18: Illustration of the two-body model and definition of the state x considered for the unscented Kalman filter (UKF).

Figure 3 .

 3 Figure 3.19: Experimental setup used to validate the performances of the tissue motion estimation algorithm when using force feedback at the base of the needle and position feedback at the needle tip.

Figure 3 Figure 3

 33 Figure 3.20: Example of measured and estimated tip position as well as the corresponding absolute position and orientation estimation errors. Three different combinations of feedback are used for the update algorithm: force and torque feedback (FT, red), tip position and orientation feedback (PO, green) and feedback from all sources (FT+PO, blue).

Figure 3

 3 Figure3.22: Example of torques measured and estimated using three different combinations of feedback for the update algorithm: force and torque feedback (FT, red), tip position and orientation feedback (PO, green) and feedback from all sources (FT+PO, blue).

Figure 3

 3 Figure 3.23: Example of measured and estimated tissue position as well as the corresponding absolute position estimation error. Three different combinations of feedback are used for the update algorithm: force and torque feedback (FT, red), tip position and orientation feedback (PO, green) and feedback from all sources (FT+PO, blue).

Figure 3

 3 Figure 3.25: Mean over time and across five experiments of the absolute error between the real and modeled tip position obtained for the different update methods and two different update rates.

  Figure 3.26: Example of measured and simulated positions of the needle tip during an insertion in gelatin while lateral motions are applied to the phantom. Five different models and update methods are used for the needle tip simulations. The measured tissue motions are shown in (a), the different tip positions in (b) and the absolute error between the measured and simulated tip positions in (c).

Figure 3 .

 3 Figure3.27: Two orthogonal views of a sequence acquired during a needle insertion in gelatin. Different models and update methods are used for the needle tip simulations. Method 1: rigid needle; method 2: flexible needle; method 3: flexible needle with extremity of the tissue spline updated from the measured tip position; method 4: flexible needle with tissue spline position updated with lateral tissue motion estimation; method 5: flexible needle with tissue spline updated with lateral tissue motion estimation and extremity from the measured tip position. The tissue spline of the different models are overlaid on the images as colored lines. Method 1 does not have any cut path and methods 2, 3, 4 and 5 are depicted in green, blue, red and yellow, respectively. The real needle can be seen in black, although it is mostly recovered by the tissue splines associated with methods 4 and 5. Overall only the tissue splines of method 4 and 5 can follow the real shape of the path cut in the gelatin.

Figure 3

 3 Figure3.28: Example of absolute error between the measured and simulated positions of the needle tip when using an update rate of 1 Hz for the estimation of the tissue motions.

Figure 3

 3 Figure 3.29: Example of tissue motions measured and estimated using the update method 4 with the position feedback obtained from cameras. Two update rates are compared: (a) fast update rate corresponding to the acquisition with cameras, (b) slow update rate simulating the acquisition with 3D ultrasound. Overall the estimations follow the real motions of the tissues.

Figure 3

 3 Figure3.31: Position of the tip in the 3D ultrasound volume obtained by manual segmentation and using different needle tracking methods. The insertion is performed along the y axis of the probe while the lateral motion of the tissues is applied along the z axis. One tracking method is initialized without model of the needle (blue), one is initialized from a model of the needle that does not take into account the motions of the tissues (green) and one is initialized from a model updated using the tissue motion estimation algorithm presented in section 3.5.2 (red).

Figure 3

 3 Figure 3.32: Illustration of the needle tracking in two orthogonal cross sections of a 3D ultrasound volume acquired near the end of the insertion. The result of the tracking initialized without model is represented by the blue curve, the tracking initialized from a non updated model by the green curve and the tracking initialized from an updated model by the red curve. Without information on the needle motion (blue curve), the tracking fails and get stuck on an artifact occurring at the surface of the tissues (blue arrow). Updating only the needle base leads to a initialization of the tracking around another bright structure that is tracked instead of the real needle (green curve). Taking into account the tissue motions allows a better initialization between two acquisitions, such that the tracking can find the needle (red curve).

  Figure 4.1: Illustration of the different kinds of flexible needle steering methods: (a) tip-based steering of a needle with asymmetric tip (b) base manipulation of a needle with symmetric tip (c) base manipulation of a needle with asymmetric tip.

  (a) Classical hierarchical formulation (b) Singularity robust formulation

Figure 4 . 2 :

 42 Figure4.2: Illustration of the task function framework in the case of two near incompatible tasks (n = 2) and a control vector with two components v x and v y (m = 2). Each E i is the set of control inputs for which the task i is fulfilled. Each S i is the input vector obtained using a single task i in the classical formulation (4.6). C is the input vector obtained using both tasks in the classical formulation (4.6). The same input vector C is obtained using the hierarchical formulation (4.13). Each R i is the input vector obtained using the singularity robust formulation (4.16) when the task i is given the highest priority. The contributions due to tasks 1 and 2 are shown with blue and red arrows, respectively, when the task 1 is given the highest priority and with green and yellow arrows, respectively, when the task 2 is given the highest priority.

Figure 4

 4 Figure 4.3: Illustration of the different needle base poses used to estimate the Jacobian matrices associated to the different features.

  Figure 4.3 shows an illustration of the frames corresponding to each r i . Using the first order of the Taylor expansion we then have s(r i ) s(r) + δtJ s v i . (4.25)

Figure 4 . 4 :

 44 Figure 4.4: Illustration of different geometric features that can be used to define task functions for the general targeting task.

Figure 4 . 6 :

 46 Figure 4.6: Picture of the setup used to test the hybrid base manipulation and duty-cycling controller.

Figure 4 . 7 :

 47 Figure 4.7: Final views of the front camera at the end of 4 insertions with different controls. The crosses represent the target. (a) Straight insertion with an initially aligned target: the target is missed due to tip deflection. (b) Duty-cycling control with a target shifted 1 cm away from the initial needle axis: duty-cycling control is saturated and the target is missed due to insufficient tip deflection. The target can be reached in both cases using the hybrid control framework ((c) aligned target and (d) shifted target).

Figure 4 . 8 :

 48 Figure 4.8: Measure of the lateral distance between the needle tip axis and the target during 4 insertions with different controls. (a) Straight insertion with an initially aligned target: the target is missed due to tip deflection.(b) Duty-cycling control with a target shifted 1 cm away from the initial needle axis: duty-cycling control is saturated and the target is missed due to insufficient tip deflection. The target can be reached in both cases using the hybrid control framework ((c) aligned target and (d) shifted target). In each graph, the purple sections marked "DC" correspond to duty-cycling control and the red sections marked "BM" correspond to base manipulation.

Figure 4

 4 Figure 4.9: Picture of the setup used to test the performances of the different safety tasks.

  (a) Initial state, Front camera (b) Initial state, side camera (c) Final state, front camera (d) Final state, side camera

Figure 4 . 10 :

 410 Figure 4.10: Views of the front and side cameras at the beginning and end of one experiment. The green line represents the needle segmentation and the target set for the controller is represented by the red cross.

Figure 4 . 11 :

 411 Figure 4.11: Measure of the lateral distance between the needle tip axis and the target during the insertions. Each graph shows a set of five insertions performed using one specific kind of safety task. Measures are noisy at the beginning of the insertion due to the distance between the needle tip and the target.

  Bending energy reduction Base axis / insertion point angle reduction

Figure 4 .

 4 Figure 4.12: Mean value of the final distance between the needle tip axis and the target. The mean is taken across the five experiments for each kind of safety task.

Figure 4 . 13 :

 413 Figure 4.13: Value of the distance between the needle and the initial position of the insertion point at the tissue surface during the insertions. Each graph shows a set of five insertions performed using one specific kind of safety task.

  Base axis / insertion point angle reduction

Figure 4 . 14 :

 414 Figure 4.14: Mean value of the distance between the needle and the initial position of the insertion point at the tissue surface. The mean is taken over time and across the five experiments for each kind of safety task.

Figure 4 . 15 :

 415 Figure 4.15: Value of the energy of bending stored in the needle during the insertions. Each graph shows a set of five insertions performed using one specific kind of safety task.

Figure 4 . 16 :

 416 Figure 4.16: Mean value of the energy of bending stored in the needle. The mean is taken over time and across the five experiments for each kind of safety task.

Figure 4 . 17 :

 417 Figure 4.17: Value of the angle between the needle base axis and the initial position of the insertion point at the tissue surface during the insertions. Each graph shows a set of five insertions performed using one specific kind of safety task.

Figure 4 .

 4 Figure 4.18: Mean value of the angle between the needle base axis and the initial position of the insertion point at the tissue surface. The mean is taken over time and across the five experiments for each kind of safety task.

  Estimated lateral distance using needle model

Figure 4 .

 4 Figure 4.19: Lateral distance between the needle tip axis and the target during a controlled insertion of the needle while lateral motions are applied to the phantom. (a) distance measured using the tracking of the needle, (b) distance estimated using the needle model. Two insertions are performed without update of the model to account for tissue motions (blue and green lines) and two insertions are performed while the model is fully updated (red and black lines).

Figure 4 .

 4 Figure 4.20: State of two needle models overlaid on the camera views at the end of a needle insertion. The blue cross represents the virtual target.Yellow and blue lines are, respectively, the needle and tissue spline curves of a model updated using only the pose feedback of the needle manipulator, such that the position of the tissue spline (blue) is not updated during the insertion. Red and green lines are, respectively, the needle and tissue spline curves of a model updated using the pose feedback of the needle manipulator and the visual feedback, such that the position of the tissue spline (green) is updated during the insertion.

  Targeting performances: The lateral distance between the axis of the measured needle tip and the target during the insertions are shown in Fig.4.22. Two cross sections of the US volume acquired at the end of the

Figure 4 .

 4 Figure 4.22: Measure of the lateral distance between the needle tip axis and the target during the insertions. Insertions are performed either in gelatin phantom or in porcine liver embedded in gelatin. Highest task priority is given to either the targeting or the safety tasks.

  (a) Gelatin, targeting tasks with highest priority (b) Gelatin, safety task with highest priority (c) Porcine liver, targeting tasks with highest priority (d) Porcine liver, safety task with highest priority

Figure 4 .

 4 Figure 4.23: Cross sections of an ultrasound volume at the end of the insertion for different experimental conditions. The result of the needle tracking is overlaid as a red curve and the interaction model is projected back in the two cross sections with the needle spline in blue and the tissue spline in yellow. The target is shown as a red cross. The green dashed lines indicates the surface of the tissues.

Figure 4 .

 4 Figure 4.24: Distance between the needle shaft and the initial position of the insertion point at the surface during the insertions. The graphs show the value of the distance measured in the acquired ultrasound volume or estimated from the model. Insertions are performed either in gelatin phantom or in porcine liver embedded in gelatin. Highest task priority is given to either the targeting or the safety tasks.

Figure 5

 5 Figure 5.2: Illustration of the performance of the target tracking algorithm.The motion described by (5.5) is applied to the gelatin phantom with a period T = 5s. The global mean tracking error is 3.6 mm for this experiment. However it reduces to 0.6 mm after compensating for the delay of about 450 ms introduced by the data acquisition.

Figure 5 . 4 :

 54 Figure 5.4: Picture of the setup used to compare the force exerted at the needle base using different configurations for the insertion.

  Figure 5.6: Mean value of the absolute lateral force exerted at the base of the needle. The mean is taken over time and across the four experiments for each configuration.

Figure 5

 5 Figure 5.7: Picture of the setup used to perform needle insertions toward a target embedded in an ex-vivo bovine liver while compensating for lateral motions of the phantom.

  Figure 5.9: Measures during an insertion in a bovine liver embedded in gelatin: (a) Measure of the tissue motions from the UR5 odometry, (b) measure of the needle tip position from the electromagnetic tracker and measure of the target position from the tracking in 2D ultrasound, (c) measure of the lateral distance between the target and the needle tip. Overall the target can be reached with good accuracy even if the tissues are moving.

Figure 5 . 10 :

 510 Figure 5.10: Target tracking in ultrasound images during a needle insertion in a bovine liver embedded in gelatin. The boundaries of the target are not always clearly visible. The needle being inserted can slightly be seen coming from the right.

Figure 5 . 11 :

 511 Figure 5.11: Target tracking in ultrasound images during a needle insertion in a gelatin phantom. The needle can be seen coming from the right.

Figure 5 .Figure 5 . 13 :

 5513 Figure5.12: Slice extracted from a high resolution 3D ultrasound volume acquired at the end of an insertion in the gelatin phantom. The needle is coming from the right and the needle tip is shown with a red cross. The line on the left corresponds to a wooden stick used to maintain the spherical target during the conception of the phantom.

d

  Position of the base of the needle p 0,i Rest position of a virtual spring p N,i Needle point associated to a virtual spring T tip Normal torque at the needle tip x, y, z Generic axes of a frame χ i Characteristic function of a curve defining its domain of definition φ Angle between the wheels of a bicycle model Π j Projector onto a virtual spring plane P i v t Needle tip translation velocity v ins Needle insertion velocity θ Orientation of the wheel of a kinematic model {F b } Frame of the needle base {F t } Frame of the needle tip a Length of the bevel along the needle axis b Length of the face of a bevel d in Inner needle diameter d out Outer needle diameter E Needle Young's modulus E N Bending energy stored in the needle E T Deformation energy stored in the tissues I Second moment of area of the needle section Stiffness per unit length of the interaction between the needle and the tissues K nat Natural curvature of the trajectory of an asymmetric needle tip u k Control input vector at time index k w Process noise vector for Bayesian filtering W k Process noise matrix of a linearized system for Kalman filtering w k Process noise vector at time index k x State vector for Bayesian filtering x, y, z Generic axes of a frame x a Augmented state for unscented Kalman filtering x k State vector at time index k y Measure vector for Bayesian filtering y k Measure vector at time index k δ Dirac delta function δφ Angular displacement of the ultrasound transducer of a wobbling probe between the beginning of two frame acquisitions Binary variable indicating the direction of sweeping of the ultrasound transducer of a wobbling 3D probe d Estimated unit vector tangent to a point along a needle f b Estimated lateral force exerted at the base of a needle pj Estimated position of a point along a needle tb Estimated lateral torque exerted at the base of a needle xk State estimate after the update step for Bayesian filtering xk|k-1 State estimate after the prediction step for Bayesian filtering ŷk Measure estimate after the prediction step for Bayesian filtering λ Wavelength of an ultrasound wave . Floor operator X i Particle for a particle filtering or sigma point for unscented Kalman filtering Y i Measure vector associated to a sigma point for unscented Kalman filtering 250 LIST OF SYMBOLS φ Angle between the center and current orientation of the ultrasound transducer of a 3D wobbling probe φ Phase of the tissue motion for the breathing motion profile ρ Mass density of a medium θ Angle of a rotation associated to the angle-axis rotation vector θu ỹk Innovation vector for Bayesian filtering × Cross product operator between two vectors {F b } Frame of the needle base {F t } Frame of the needle tip {F w } Fixed reference frame associated to a robot atan2(y, x) Multi-valued inverse tangent operator b Amplitude of the 1D tissue motion for the breathing motion profile c Speed of sound in soft tissues 1540 m.s -1 Distance between an interface in the tissues and the ultrasound transducer d o Acquisition depth of an ultrasound probe f Frequency of an ultrasound wave f s Sampling frequency of the radio-frequency signal g Generic probability density function I post Post-scan image I pre Pre-scan image J Cost function used for the needle tracking J 1 , J 2 , J 3 , J 4 Sub-cost functions used for the needle tracking K Bulk modulus of a medium k Time index for Bayesian filtering L Length of a polynomial curve l d Curvilinear coordinate of point along the needle L d , L n , L t Lateral integration distances for the needle tracking sub-costs 251 LIST OF SYMBOLS l j Curvilinear coordinate of point along the needle L p Distance between two piezoelectric elements along an ultrasound transducer L s Distance between samples along a scan line M Number of points along the needle taken as measures for unscented Kalman filtering m 1D breathing motion profile applied to the tissues N Number of control points defining the polynomial curve for the needle tracking n Coefficient tuning the shape of the motion for the breathing motion profile n Number of segments in a spline of the two-body model N ν Dimension of the measurement noise vector for Bayesian filtering N f Number of frames acquired during a sweeping motion of the ultrasound transducer of a 3D wobbling probe N l Number of scan lines N p Number of particles of a particle filter n p Number of piezoelectric elements of an ultrasound transducer N s Number of samples acquired along a scan line N u Dimension of the control input vector for Bayesian filtering N w Dimension of the process noise vector for Bayesian filtering N x Dimension of the state vector for Bayesian filtering N y Dimension of the measure vector for Bayesian filtering p Generic probability density function R Radius of curvature of a convex transducer r Polynomial order of spline segments R m Radius of the circular trajectory described by the ultrasound transducer of a 3D wobbling probe r N Radius of the needle expressed in voxels in an ultrasound volume λ δ Positive control gain for the task associated to the distance between the rest position of the insertion point and the needle point at the surface of the tissues λ γ Positive control gain for the task associated to the angle between the needle base axis and the rest position of the insertion point λ σ Positive control gain for the task associated to the angle between the bevel cutting edge and a target λ θ Positive control gain for the task associated to the angle between the needle tip axis and a target λ d Positive control gain for the task associated to the distance between the needle tip axis and a target λ δ Positive control gain for the task associated to the vector between the rest position of the insertion point and the needle point at the surface of the tissues λ δm Positive control gain for the task associated to the mean deformation of the tissues along the needle shaft λ E N Positive control gain for the task associated to the bending energy stored in the needle ω z,max Maximal rotation velocity around the needle axis σ Angle between the bevel cutting edge and a target σ i Singular value of a matrix τ i Singular value of the pseudo-inverse of a matrix v t Translation velocity of the needle tip v t,z Translation velocity of the needle tip along its axis v tip Scalar insertion velocity of the needle tip θ Angle between the needle tip axis and a target θ DC Angle of rotation of the tip during one cycle of duty-cycling control J Estimation of the Jacobian matrix J {F b } Frame of the needle base {F t } Frame of the needle tip {F w } Fixed reference frame associated to a robot 256 LIST OF SYMBOLS atan2(y, x) Multi-valued inverse tangent operator d Distance between the needle tip axis and a target DC Duty cycle in duty-cycling control E N Bending energy stored in the needle i Level of priority of a task K ef f Effective curvature of the trajectory of an asymmetric needle tip during duty-cycling control K nat Natural curvature of the trajectory of an asymmetric needle tip L N Length of the spline curve representing the needle model L DC Insertion length of a cycle during duty-cycling control L f ree Length of the needle that is outside the tissues L ins Length of the insertion phase in duty-cycling control L rot Length of the rotation phase in duty-cycling control L thres Threshold length before the addition of a tissue spline segment in the two-body model L thres Threshold length between the addition of two successive virtual springs m Dimension of the control input vector n Dimension of the task vector n Number of segments in a spline of the two-body model r Polynomial order of spline segments t Generic time x 0 , y 0 , z 0 Components of the rest position of the insertion point in the frame of the needle base x t , y t , z t Components of the position of a target in the frame of the needle tip Chapter 5 . d Subscript used to indicate the desired value of a quantity 0 3×5 3 by 5 null matrix a Initial position of the tissues for the breathing motion profile b Amplitude of the tissue motion for the breathing motion profile c N Spline curve representing the needle e Task vector f l Lateral force exerted at the base of the needle I 3 3 by 3 identity matrix J Generic Jacobian matrix relating the variation of the task vector with respect to the control inputs J γ Jacobian matrix associated to the angle between the needle base axis and the rest position of the insertion point J σ Jacobian matrix associated to the angle between the bevel cutting edge and a target J f Jacobian matrix associated to the lateral force exerted at the base of the needle J ω U R ,z Jacobian matrix associated to the rotation velocity of the robot around the needle axis J v U R ,z Jacobian matrix associated to the translation velocity of the tip of the needle insertion device along the needle axis J v U R Jacobian matrix associated to the translation velocity of the tip of the needle insertion device J vt,z Jacobian matrix associated to the translation velocity of the needle tip along its axis J v N ID Jacobian matrix associated to the translation velocity of the translation stage of the needle insertion device J θ Jacobian matrix associated to the angle between the needle tip axis and a target m Breathing motion profile applied to the tissues v r Control inputs vector of the robotic system consisting of the UR3 robot and the needle insertion device v N ID Control inputs vector of the needle insertion device v U R Control inputs vector of the UR3 robot 258 LIST OF SYMBOLS x, y, z Generic axes of a frame γ Angle between the needle base axis and the rest position of the insertion point λ γ Positive control gain for the task associated to the angle between the needle base axis and the rest position of the insertion point λ σ Positive control gain for the task associated to the angle between the bevel cutting edge and a target λ θ Positive control gain for the task associated to the angle between the needle tip axis and a target λ f Positive control gain for the task associated to the lateral force exerted at the base of the needle ω z,max Maximal rotation velocity around the needle axis ω N ID Rotation velocity of the rotation stage of the needle insertion device ω U R ,z Rotation velocity of the robot around the needle axis σ Angle between the bevel cutting edge and a target v U R ,z Translation velocity of the tip of the needle insertion device along the needle axis v U R Translation velocity of the tip of the needle insertion device v t,z Translation velocity of the needle tip along its axis v tip Scalar insertion velocity of the needle tip v N ID Translation velocity of the translation stage of the needle insertion device θ Angle between the needle tip axis and a target {F b } Frame of the needle base {F t } Frame of the needle tip {F w } Fixed reference frame associated to a robot E Needle Young's modulus I Second moment of area of the needle section L thres Threshold length before the addition of a tissue spline segment in the two-body model Résumé Le guidage robotisé d'une aiguille a été le sujet de nombreuses recherches ces dernières années afin de fournir une assistance aux cliniciens lors des procédures médicales d'insertion d'aiguille. Cependant le contrôle précis et robuste d'un système robotique pour l'insertion d'aiguille reste un grand défi à cause de l'interaction complexe entre une aiguille flexible et des tissus ainsi qu'à cause de la difficulté à localiser l'aiguille dans les images médicales.

Table 1 .

 1

	Needle type	Chiba biopsy needle	Chiba biopsy stylet
	Reference	Angiotech MCN2208	Aurora Needle 610062
	Young's modulus 200 GPa	200 GPa
	Outer diameter	22G (0.7 mm)	23.5G (0.55 mm)
	Inner diameter	0.48 mm	0.5 mm
	Length (cm)	12.6	from 0.8 to 10.8
	Tip type	Chiba	Chiba
	Tip angle	25 •	25 •

1: Characteristics of the needles used in the experiments. The lengths are calculated from the base of the needle holder to the needle tip.

Table 3 .

 3 1: Mean over time and across five experiments of the absolute error between the real and modeled tip position obtained for the different update methods and two different update rates.

		Absolute position error (mm)
	Update rate	30 Hz	1 Hz
	Method 1	5.9±3.9	5.9±3.9
	Method 2	6.1±3.0	6.1±3.0
	Method 3	2.1±1.6	1.9±1.5
	Method 4	0.6±0.3	0.9±0.5
	Method 5	0.4±0.2	0.7±0.5

Table 5 .

 5 3: Summary of the lateral force measured at the base of the needle during the insertions performed in a gelatin phantom and a bovine liver embedded in gelatin. The mean and standard deviation of the lateral force are calculated over time.

	Phantom Max	Force (mN) Mean Global mean
	Gelatin Liver	630 870 189 ± 119 154 ± 92 494 162 ± 88 753 217 ± 135 773 268 ± 176 1022 137 ± 127 538 61 ± 72 601 41 ± 64 626 64 ± 61 474 136 ± 127	198 ± 133 88 ± 103

3D US probe

• The second task controls the bevel orientation via the angle σ, as defined by (4.51), (4.54) and (4.56). The maximal rotation speed ω z,max is set to 60 • .s -1 and the gain λ σ is set to 10 (see (4.56)) such that the maximal rotation velocity is used when the bevel orientation error is higher than 6 • .

• The third task is the safety task used to reduce the tissue stretch at the surface δ, as defined by (4.61), (4.62) and (4.63). The control gain λ δ is set to 1.

Note that in a general clinical context it is not always possible to see the insertion point at the tissue surface due to the configuration of the probe with respect to the insertion site. So we choose here to use the estimation of δ using the needle model instead of the real measure as an input of the safety task.

Two sets of priority levels are tested. In the first set, the two targeting tasks (first and second tasks) have the same priority and the safety task (third task) has a lower priority. The final velocity screw vector v b applied

Motion compensation using force feedback

In previous sections we have defined a way to use force feedback in our needle steering framework as well as a method to track a moving target in 2D ultrasound (US) images. Therefore, in this section we present the results of experiments that we conducted to test our control framework in the case of a needle insertion performed under tissue motions.

Force sensitivity to tissue motions

We first propose to compare the sensitivity of the force measurements depending on the configuration of the needle. Two configurations are mostly used to perform robotic needle insertions. The first one is mainly used to performed base manipulation and consists in holding the needle by its base, leaving a part of the body of the needle outside the tissues during the insertion. The second configuration is mainly used to performed tip-based steering. The needle is then usually maintained in an insertion device such that only the part outside of the device can bend. The device is placed near the surface such that the needle is directly inserted inside the tissues, with no intermediate length left free to bend between the device and the tissues.

In the following we perform needle insertions using different configurations and compare the interaction forces measured at the base of the needle.

Experimental conditions (setup in the Netherlands): We use the needle insertion device (NID) attached to the UR3 robot arm. The biopsy needle with the embedded electromagnetic (EM) tracker is placed inside the NID and is inserted in a gelatin phantom. The ATI force torque sensor is used to measure the interaction efforts exerted at the base of the needle. A picture of the setup is shown in Fig. 5.4.

The position of the EM tracking system is registered in the frame of the UR3 robot before the experiments using the method that was presented in section 3.6.1. The force torque sensor is also calibrated beforehand to remove the sensor biases and the effect of the weight of the NID in order to reconstruct the interaction forces applied to the base of the needle (see Appendix A).

A fixed virtual target is defined just before the beginning of the insertion such that it is at a fixed position in the initial frame of needle tip.

We use the two-body model presented in section 2.4.2 with polynomial needle segments of order r = 3 to represent the part of the needle that is outside of the NID, from the frame {F b } depicted in Fig. 5.4 to the needle tip. We fix the length of the needle segments to 1 cm, resulting in n = 1 segment of 8 mm when the needle is retracted to the maximum inside the NID and n = 11 segments with the last one measuring 8 mm when the needle is fully outside of the NID. We use a rather hard phantom, such that we set the

List of Publications

Appendix A

Force sensor calibration

This appendix presents the registration process and the computation method, used in the experiments of chapters 3 and 5, to retrieve the interaction forces and torques applied at the base of the needle without the gravity component due to the mass of the needle insertion device.

The force f ∈ R 3 measured by the sensor can be expressed according to

where m d is the mass of the needle insertion device (NID), g ∈ R 3 is the gravity vector, b f ∈ R 3 is the sensor force bias and f ext ∈ R 3 is the rest of the forces applied to the sensor, with each vector defined in the sensor frame. The torque t ∈ R 3 measured by the sensor can be expressed similarly according to

where × denotes the cross product operator, c d ∈ R 3 is the position of the center of mass of the NID, b t ∈ R 3 is the sensor torque bias and t ext ∈ R 3 is the rest of the torques applied to the sensor, with again each vector defined in the sensor frame. Note that f ext and t ext correspond to the contribution of the interaction forces and torques that we want to measure. Let us define g w the gravity vector expressed in the world reference frame and w R f ∈ SO(3) the rotation from the world frame to the force sensor frame such that

During the insertion procedure, the contribution of the gravity and the biases can be removed depending on the pose of the NID to isolate the interaction forces. Then,

The interaction forces f b ∈ R 3 and torques t b ∈ R 3 applied to the base of the needle can then be expressed in the needle base frame according to

where f R b ∈ SO(3) and f T b ∈ R 3 are, respectively, the rotation and translation from the sensor frame to the needle base frame. In practice only the orientation w R e ∈ SO(3) of the end effector of the UR3 is known thanks to the robot odometry such that w R f is actually computed according to Noting g i the gravity vector associated to i th orientation of the UR3 end effector, b f and m d can first be computed to minimize the cost function J f defined as

which leads after calculations to

Then b t and c d can be computed to minimize the cost function J t defined as

which leads after calculation to

with

where g i,x , g i,y and g i,z are the components of g i .

LIST