Continuous Curvature Path Planning for a

Continuous Curvature Path Planning for a Neurosurgical Flexible Probe

Sophia Bano

Heriot-Watt University, United Kingdom Universitat de Girona, Spain Universite de Bourgogne, France Imperial College London, United Kingdom

A Thesis Submitted for the Degree of MSc Erasmus Mundus in Vision and Robotics (VIBOT) · 2011 ·

Abstract With recent advances in medical imaging modalities, minimally invasive methods for performing percutaneous procedures are becoming the choice of many neurosurgeons around the world. In contrast to conventional Open Neurosurgery, Minimally Invasive Neurosurgery gives benefits of reduced trauma, less pain and shorter recovery time, to the patient. Minimally invasive neurosurgery requires the insertion of surgical tools, such as, probes, needles or electrodes inside the brain, precisely targeting tumors, lesions, and anatomical structures, based on pre-operative planning on CT/MRI images, while avoiding obstacles. Obstacles represent sensitive tissue, nerves or arteries, which should not be cut by surgical tools. Currently, path planning in imageguided minimally invasive neurosurgery is usually performed manually on two dimensional cross-sections of the patient’s pre-operative CT/MRI brain images and is surgeon-dependent and may not be optimal. Therefore, there is a need for a pre-operative path planning method, which can guide surgeons in generating an optimal path for neurosurgery. In this thesis, path planning methods were developed, for steering a neurosurgical flexible probe (STING - Soft Tissue Intervention Neurosurgical Guide), which is under development at the Mechatronics in Medicine Laboratory of Imperial College London, UK. The neurosurgical flexible probe is capable of steering in two dimensional space and is modelled as a nonholonomic robot. Mechanical constraints of the flexible probe impose a minimum radius of curvature constraint on the path, thus bounding its curvature. Further, the probe kinematics requires the curvature to be continuous and smooth. Path planning methods were developed based on the mechanical constraints of the flexible probe, in order to compute optimal path with minimum risk to the patients. A gradientbased method which made use of the optimization of a curvature model for path planning of nonholonomic robot was implemented, to generate continuous curvature paths. A samplingbased method of Rapidly-exploring Random Tree (RRT) combined with reachability guided heuristic was applied to get paths with bounded curvature. With the integration of gradient and sampling-based methods, a locally smooth path planning approach was developed. Finally, a globally smooth path planner was designed by combining the sampling based method with a smoothing approach. The smoothing approach was developed by adapting bézier mathematics. A path optimization scheme was also developed, which took into account factors, such as minimum damage to the tissue, maximum clearance from obstacles and minimum risk to the patient. Results showed that the globally smooth path planner was more efficient and effective than the other three developed path planners, namely, the gradient based, sampling based and locally smooth path planner and it satisfied the nonholonomic constraint of the neurosurgical flexible probe.

The true sign of intelligence is not knowledge but imagination. Albert Einstein

2

Contents Acknowledgments

viii

1 Introduction

1

1.1

Minimally Invasive Surgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2

Medical Motivation for Steerable Needles . . . . . . . . . . . . . . . . . . . . . .

2

1.3

Motivation for Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

1.4

Goal of the Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

1.4.1

. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

Dissertation Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

1.5

Problem Formulation

2 Background Study and Literature Review 2.1

2.2

2.3

6

STING: A Soft-Tissue Intervention and Neurosurgical Guide . . . . . . . . . . .

7

2.1.1

Bio-inspired Programmable Bevel concept . . . . . . . . . . . . . . . . . .

7

2.1.2

Nonholonomic Nature of the Flexible Probe . . . . . . . . . . . . . . . . .

8

2.1.3

System Architecture of the Flexible probe . . . . . . . . . . . . . . . . . .

9

Path Planning Algorithms Overview . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.2.1

Deterministic methods for Path Planning . . . . . . . . . . . . . . . . . . 11

2.2.2

Sampling based Path Planning Algorithms . . . . . . . . . . . . . . . . . 12

Towards Path Planning of Nonholonomic Robots . . . . . . . . . . . . . . . . . . 14 2.3.1

Dubins’ Curves and its Variants . . . . . . . . . . . . . . . . . . . . . . . 14 i

2.4

2.3.2

Gradient Method for Path Planning . . . . . . . . . . . . . . . . . . . . . 15

2.3.3

Gradient Method with Obstacle Avoidance . . . . . . . . . . . . . . . . . 20

2.3.4

Reachability Guided Rapidly Exploring Random Tree (RG-RRT) . . . . . 22

2.3.5

RRT with Continuous-Curvature Path-Smoothing Algorithm . . . . . . . 27

Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3 Methods 3.1

3.2

3.3

3.4

3.5

3.6

30

Gradient based method for Continuous Curvature Path Planning . . . . . . . . . 30 3.1.1

Normalization of Posture Coordinate . . . . . . . . . . . . . . . . . . . . . 32

3.1.2

Designing Cost Functions for Obstacle Avoidance . . . . . . . . . . . . . . 33

3.1.3

Strength and Weaknesses . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

Path Planning using Rapidly Exploring Random Trees . . . . . . . . . . . . . . . 36 3.2.1

RG-RRT based Path Planner Algorithm . . . . . . . . . . . . . . . . . . . 36

3.2.2


Locally Smooth Path Planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.3.1

Integration of RG-RRT with Continuous Curvature Path Planner

. . . . 39

3.3.2


Smooth Path Planning Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3.4.1

Bézier Curve Theory and Mathematics . . . . . . . . . . . . . . . . . . . . 41

3.4.2

Globally Smooth Path Planner . . . . . . . . . . . . . . . . . . . . . . . . 43

3.4.3


Optimization of Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 3.5.1

Optimization Criteria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

3.5.2

Total Optimal Cost Formulation . . . . . . . . . . . . . . . . . . . . . . . 46

Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4 Results and Comparison

48

4.1

Brain Risk-Map and Configuration Space . . . . . . . . . . . . . . . . . . . . . . 48

4.2

Testing Path Planner on Brain Risk-map . . . . . . . . . . . . . . . . . . . . . . 49 ii

4.2.1

Results for Gradient based method for Continuous Curvature Path Planning 49

4.2.2

Results for RG-RRT based planner . . . . . . . . . . . . . . . . . . . . . . 51

4.2.3

Locally Smooth Path Planner . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.2.4

Result for Globally Smooth Path Planner . . . . . . . . . . . . . . . . . . 54

4.3

Comparison of Developed Path Planning Methods . . . . . . . . . . . . . . . . . 55

4.4

Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

5 Conclusions

58

5.1

Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

5.2

Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

Bibliography

64

iii

List of Figures 1.1

(a) Testing of the flexible probe in tissue phantom (i.e.

gelatine), (b) The

grayscale workspace, which is the risk-map for brain. . . . . . . . . . . . . . . . .

4

2.1

Model of flexible steerable probe along with its control mechanism . . . . . . . .

7

2.2

(a) Flexible probe prototype composed of four interlocked segments, (b) Programmable bevel tip concept: the offset between two interlocked segments of a flexible probe determines the steering direction of the tip

2.3

. . . . . . . . . . . . .

8

(a) Representation of the reciprocal motion and the resistive forces acting on the bevel tip of the probe penetrating the gelatine, (b) Example showing flexible probe following a pre-defined path in tissue phantom (i.e. gelatine) . . . . . . . .

2.4

Notation comparison between the flexible probe (left) and the conventional bicycle model (right) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2.5

9

9

Block Diagram of the system architecture. It is split into two main blocks, namely, path planning and path following, where path planning is the problem under consideration in this thesis. . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.6

The basic motion planning problem is conceptually very simple using C-space ideas. The task is to find a path from qI to qG in Cf ree . The entire blob represents C = Cf ree ∪ Cobs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.7

RRT Extend operation: a new state qnew is placed along the segment connecting nearest node qnear with the random sample qrand . . . . . . . . . . . . . . . . . . 14 iv

2.8

Dubins curve trajectory for two words are shown in W = R2 . . . . . . . . . . . 16

2.9

Reachability criterion in 2D and 3D configuration space . . . . . . . . . . . . . . 25

2.10 Geometry of constructing circular arc . . . . . . . . . . . . . . . . . . . . . . . . 27 2.11 Path planning using RRT algorithm and path smoothing using G2CBS algorithm 28 3.1

Flow diagram for Cubic Curvature Path Planner . . . . . . . . . . . . . . . . . . 31

3.2

Method 2: Continuous Curvature Path Planning obtained by using Lobs cost for obstacle avoidance. (a) shows cubic polynomial path in red and 4th order path incorporating obstacle avoidance in green. . . . . . . . . . . . . . . . . . . . . . . 34

3.3

Method 2: Continuous Curvature Path Planning obtained by using Lef f cost for obstacle avoidance. (a) shows cubic polynomial path in red and 4th order path incorporating obstacle avoidance in green. . . . . . . . . . . . . . . . . . . . . . . 35

3.4

Flow diagram for Reachability Guided Rapidly-exploring Random Tree (RG-RRT) 37

3.5

Comparsion of basic RRT with RG-RRT. First row indicates results obtained by using basic RRT, while second row indicates result obtained by RG-RRT

. . . . 38

3.6

Method 1: Reachability Guided Rapidly Exploring Random Tree (RG-RRT)

. . 39

3.7

RG-RRT with CCT flow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

3.8

Method 3: Locally Smooth Path Planner formed by the integration of RG-RRT and Continuous Curvature polynomial path generator . . . . . . . . . . . . . . . 40

3.9

Various Bézier segments of degree 2 to 5. The control points are shown with red dots, and the control polygons(line segments connecting the control points) are shown with blank lines. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

3.10 Flow of globally smooth path planner, which involves RG-RRT for path planning and Bézier curve for path smoothing . . . . . . . . . . . . . . . . . . . . . . . . . 43 3.11 Method 4: Globally Smooth Path Planner developed using Bézier Curve . . . . . 44 4.1

The coronal section of risk-map of brain, with all six levels of risk labelled. The configuration space for flexible probe is extracted from the Risk-map . . . . . . . 49 v

4.2

Gradient based path planner results showing the effect of varying target orientation θ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

4.3

Results for optimization of path obtained by applying gradient based approach of path planning, which is obtained by varying target orientation value . . . . . . 50

4.4

Result obtained using RG-RRT based path planner. Random branches are shown in blue, nodes are shown by red dots while the computed final path is shown in green. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

4.5

Optimzation of path for RG-RRT based planner obtained by generating multiple paths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

4.6

Results obtained by applying Locally Smooth Path Planner on coronal section of brain Risk-map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.7

Globally smooth path planner results, where the generated path(in green), control points(red dots) and bézier Polygon segments(blue) are clearly shown in zoomed version . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

4.8

Results for optimization of path obtained by applying Globally smooth path planner by generating multiple paths . . . . . . . . . . . . . . . . . . . . . . . . . 54

vi

List of Tables 2.1

Termination Conditions for Gradient Method . . . . . . . . . . . . . . . . . . . . 18

2.2

Summary of existing Nonholnomic Path Planning Methods . . . . . . . . . . . . 29

4.1

Comparison of Developed Path Planning Methods . . . . . . . . . . . . . . . . . 55

vii

Acknowledgments This thesis work would not have been possible without the support of many people. I am truly grateful to my supervisor Dr. Ferdinando y Baena for providing me the opportunity to work at Mechatronics in Medicine (MiM) Laboratory. I really appreciate his continuous support and encouragement. I am sincerely thankful to my co-supervisor Dr. Seong Young Ko, for his technical advice and guidance. Special thanks to Stuart and all my colleagues at MiM Lab, for helping me out and sharing good times. I would like to take this opportunity to express my deep gratitude to Prof. Fabrice Meriaudeau, David Fofi, Yohan Fougerolle, Yvan Petillot, Robert Marti, Marc Carreras and all my professors, who taught me during VIBOT course and opened my thoughts to new horizons of knowledge. I feel very lucky to be part of this VIBOT family. I am also humbly gratified by all the people who by different ways are contributing to VIBOT Master Programme. I am heartily thankful to all Vibotian friends with whom I shared these two years. It has brought an unforgettable aura full of enjoyable time, open discussions and everlasting friendships. Finally, I owe a special praise to my family for their understanding and endless love. I am thankful to my mother, without her encouragement and prayers it would have been impossible for me to finish this work.

viii

Chapter 1

Introduction With recent advances in medical imaging modalities such as MRI, ultrasound, and x-ray fluoroscopy, real-time patient-specific information is now available to physicians and provides assistance to them while carrying out complex surgical procedures. These medical imaging modalities are helpful while performing percutaneous procedures like extracting tissue samples for biopsies, injecting drugs for anesthesia, or implanting radioactive seeds for brachytherapy cancer treatment. Thus, during minimally invasive procedures, the useful information, such as, three dimensional displays, planning, real-time intraoperative monitoring and localization, obtained from computer assistance, proved to be important in obtaining better results.

1.1

Minimally Invasive Surgery

Minimally Invasive Surgery(MIS) involves procedures performed by entering the skin via small incisions or by a body cavity, with aim of producing least possible damage to structures while achieving the same results as if performed by open or more invasive surgery [1] [2]. Large incisions enable surgeons to see and manipulate the pathological tissue directly. The significant damage done to organs in the surgical path causes pain to the patient, entails long recovery time, and causes complications due to surgical trauma. The goal of MIS is to prevent unnecessary trauma by reducing the size of incisions. The benefits of reduced trauma, less pain and shorter recovery time, make MIS the technique of choice of many surgeons around the world. Free-hand MIS suffers from reduced dexterity, limited perception, increased error, and longer procedure time. Fortunately, contemporary computer aided technologies help reduce these limitations and enable a better access, dexterity, perception and accuracy. The MIS procedure can further be improved, by introducing a pre-operative technique for optimal path following during surgery. 1

Chapter 1: Introduction

2

A keyhole neurosurgery is a MIS technique for brain, in which a needle or a probe is inserted through a slot to the pathological tissue and is manipulated. Whenever feasible, neurosurgery should be MIS, to minimize patient discomfort, reduce damage to normal tissue, improve cosmetic results and shorten hospital stay and recovery time [1] [3]. Neurological procedures, such as, removal of pituitary tumours, treatment of intracranial aneurysms, carotid angioplasty and radiosurgery for brain tumours are performed using minimally invasive neurosurgery.

1.2

Medical Motivation for Steerable Needles

Percutaneous interventions require insertion of a needle to a specific location inside the body to extract a tissue sample, implant radioactive seed, or inject a drug [3] [4]. In all these cases, the needle tip should be inserted as close as possible to a predetermined target inside soft tissue. Thick and non-flexible needles are easily pointed to the target in the existence of a visualization system, but their manipulation causes significant pressure on the tissue. Moreover, straight needles are not suitable for following curved paths, if obstacle avoidance is required. These problems are solved by using thin and flexible needles, which causes less pain to the patient. Steerable needles resolve most of the difficulties observed by surgeons while using traditional straight, rigid needles, thereby improving targeting, enabling novel treatment methods and reducing complication rates. Steerable needles have two key properties: they are composed of a flexible material and have a bevel-tip. These properties enable steerable needles to follow curved paths through soft tissue [5]. Steerable needle are subject to nonholonomic constraint based on bevel direction and can be modelled as a car-like robot, which have constraint on their steering angle [6]. Motion planning of steerable needles is required in order to improve accuracy, minimize the uncertainties introduced by tissue deformation and for avoiding undesirable bending of needles during insertion. Needle steering in brain tissue phantom is demonstrated by B. Badie [3] and JA. Engh [7]. Motion planning and optimal path generation for a particular type of bevel tip steerable needles during surgical intervention is presented by R. Alterovitz [8] [9].

1.3

Motivation for Work

Most of the percutaneous interventions require precise localization of target, such as tumor or an anatomical structure within the brain, with a surgical tools, like, flexible needle. Preoperative CT/MRI guide such type of intervention and helps in avoiding obstacles. Obstacles represent tissues that cannot be cut by the needle, such as bone, or sensitive tissues that should not be damaged, such as nerves or arteries. A misplacement of flexible needle might lead to

3

1.4 Goal of the Project

severe medical complications [10]. Thus, there is a need to select a pre-operative safe path that can reduce any risk to the patient. Currently, path planning in image guided percutaneous intervention is usually performed manually on two dimensional cross-sections of the patient’s pre-operative CT/MRI brain images and is surgeon-dependent. This might leads toward a sub-optimal path, thus increasing risk to the patient. Therefore, there is a need for a pre-operative path planning method, which can guide surgeons in generating an optimal path for neurosurgery.

1.4

Goal of the Project

This research work focuses on Path Planning for a Neurosurgical Flexible Steerable Probe that is capable of steering in a medium such as soft tissue. The path planning algorithm is to be designed, specifically for the neurosurgical flexible probe named STING (Soft Tissue Intervention Neurosurgical Guide), which is under development at the Mechatronics in Medicine Laboratory of Imperial College London, UK. This neurosurgical flexible probe is capable of steering in two dimensions and can be modelled as a nonholonomic robot. Mechanical constraints of the flexible probe impose a minimum radius of curvature constraint on the path, thus bounding its curvature. Further, the flexible probe kinematics requires the curvature to be continuous and smooth.

1.4.1

Problem Formulation

Path planning for a mobile robot refers to a problem of obtaining the most feasible path between the defined start and goal position, while respecting the mechanical limitations of the robot and the constraints imposed by the environment. The key goal of this research work is to find an optimal path for the neurosurgical flexible probe, from a given starting point to a lesion on a two dimensional map of the brain. The following considerations must be made while developing the path planning approach: • Mechanical Constraints: The flexible steerable probe is modelled as a nonholonomic robot and has a bounded curvature range κ(s) (where s is the length of the curve), for the path it follows. Further, there is a minimum radius of curvature rmin constraint, which can be interpreted as the limited steering angle for a car-like robot. The mechanical constraints does not allow the flexible probe to change its curvature drastically, hence, there is a continuity and boundedness constraint on the derivative of the curvature κ0 (s) as well. The thickness of the flexible probe is also considerable, while forming the configuration space for the robot.

Chapter 1: Introduction

(a) Flexible probe in gelatine

4

(b) Brain Risk-map

Figure 1.1: (a) Testing of the flexible probe in tissue phantom (i.e. gelatine), (b) The grayscale workspace, which is the risk-map for brain. • Brain Risk-map: Two different workspaces has to be considered during path planning, namely , a binary workspace and a grayscale workspace. Binary workspace is generated manually for testing algorithms while grayscale workspace is a risk-map of brain provided for testing and is shown in Figure 1.1(b). In this risk-map, a risk value is assigned to each part of the brain, depending upon its dangerous. A continuous final path has to be developed by using these work spaces.

Inputs for the Path Planner The inputs for the path planning algorithm of the flexible steerable probe are: • The binary or grayscale risk-map of brain, which is later converted into configuration space by considering the thickness of the flexible probe. • The start or entry pose (position and orientation in 2D space) is defined. In case of some path planning algorithms, it is necessary to define the curvature (κ) as well, thus a start or entry posture (position, orientation and curvature) is defined in that case. • The target position or posture is to be defined (depending upon the path planning approach to be followed). Outputs from the Path Planner The path planning algorithm is expected to give the following outputs: • A continuous curvature path that is bounded by constraints on the curvature and its derivative. The generated path should also be optimal in terms of the risk value.

5

1.5 Dissertation Overview

• A cost value for the generated path, reflecting its intersection with different risk areas on the map. The generated path should satisfy the mechanical constraints induced by flexible probe and it should be continuous and bounded in terms of the curvature. The derivative of curvature should be continuous, thus, guaranteeing smooth path. Further, the path planner should be able to guide Neurosurgeon through optimal path, such that it minimize the risk to the patient, while at the same time generates shortest possible path, which can be followed during intervention based on a predefined brain risk-map. The algorithm should be of pre-operative nature and can be run offline before performing an intervention. The algorithm should output a trajectory(in the form of uniformly spaced discrete points), featuring the above mentioned properties, which can then be linked with the control part of the flexible probe for path following.

1.5

Dissertation Overview

The organization of this dissertation is as follows: • Chapter 2 presents the neurosurgical flexible probe under consideration and the literature review involved for its path planning. A comprehensive review of nonholonomic robots path planning is also reported. • Chapter 3 demonstrates the methods developed for the continuous curvature path planning of the neurosurgical flexible probe. A path optimization approach is also presented. • Chapter 4 details the testing of the developed path planning methods on the provided risk-map of the brain. A thorough comparison of all developed methods for the path planning of the neurosurgical flexible probe is also presented. • Chapter 5 concludes the dissertation and presents the possible directions for future work.

Chapter 2

Background Study and Literature Review Percutaneous procedures involving needle insertions include vaccinations, blood/fluid sampling, regional anesthesia, tissue biopsy, catheter insertion, cryogenic ablation, electrolytic ablation, brachytherapy, neurosurgery, deep brain stimulation and so forth. Thick and non-flexible needles are easily pointed to the target in the existence of a visualization system, but their manipulation causes significant pressure on the tissue. Moreover, straight needles are not suitable for following curved paths, if obstacle avoidance is required. These problems can be solved by using thin and flexible needles. Thus, a biologically inspired neurosurgical flexible steerable probe [4] [11] [12] is currently being developed at the Imperial College London, the aim of which is to access deep brain lesions with minimum damage to the patient. For such thin and flexible needles, manual trajectory planning by surgeons during minimally invasive surgery leads to sub-optimal path, increasing the risk to the patient. Therefore, a pre-operative path planning approach is required to get the optimal path. This chapter details the background study of neurosurgical flexible probe and literature review involved in path planning of the flexible probe. Section 2.1 presents the neurosurgical flexible probe under consideration and the system involve in its motion planning. Path planning algorithms and their brief overview is detailed in Section 2.2. While, algorithms specific to the path planning of nonholonomic robots are presented in Section 2.3. Summary of algorithms studied for the nonholonomic robot path planning is detailed in Section 2.4. 6

7

2.1 STING: A Soft-Tissue Intervention and Neurosurgical Guide

Figure 2.1: Model of flexible steerable probe along with its control mechanism [4]

2.1

STING: A Soft-Tissue Intervention and Neurosurgical Guide

Development of a Soft-Tissue Intervention and Neurosurgical Guide (STING) [4] [11] [12], for the purpose of accessing deep brain lesions, is in process at the Mechatronics in Medicine Laboratory of Imperial College London, UK. STING, which is refereed simply as the flexible probe, in this thesis work, is a neurosurgical needle being designed for minimally invasive neurosurgeries and is capable of following curvilinear trajectories [4]. The complete system model for the flexible probe is shown in Figure 2.1, which display the assembly of its actuators and control part.

2.1.1

Bio-inspired Programmable Bevel concept

The flexible probe design is inspired by the ovipositor of wood wasp, which is used by it to penetrate the bark of wood to deliver eggs [4] [11] [12]. The ovipositor consists of several values, which are connected by an interlocking mechanism and works using reciprocal motion. Thus, the bio-inspired flexible probe is designed to steer in the brain tissue, while avoiding sensitive anatomical parts of the brain and will be able to reach safely to the target position in the brain. Currently, the flexible probe prototype testing is being done in brain like material, i.e. gelatine and this steering is shown in Figure 2.3(b). The flexible probe consists of four interlocked segments, which are capable of sliding independently with respect to each other, as shown in Figure 2.2(a). Two functional channels are included in the probe, one for medical procedures (e.g. localized drug delievery), while, the other for an electro-magnetic sensor for tracking and localization of the probe. The offset between the segments of flexible probe leads to its bending, due to bevel tip, which is demon-

Chapter 2: Background Study and Literature Review

(a) Flexible probe prototype

8

(b) Programmable bevel tip concept

Figure 2.2: (a) Flexible probe prototype cross-section (φ = 12 mm, length = 200 mm) composed of four interlocked segments. Two functional channels are included: one for medical procedures (e.g. localized drug delivery), the other for an electro-magnetic tracking sensor required to control probe position, (b) Programmable bevel tip concept: the offset between two interlocked segments of a flexible probe determines the steering direction of the tip [6]

strated in Figure 2.2(b), thus, the probe insertion direction is a function of the offset between probe segments. Figure 2.3(a) shows the reciprocal motion and resistive forces acting on the bevel tip. The interaction between the bevel tip of each probe segment and the surrounding tissue, forces each segment to bend in a precise direction along the trajectory.

2.1.2

Nonholonomic Nature of the Flexible Probe

The flexible probe’s kinematic model is comparable to a nonholonomic robot model,i.e, a carlike robot, the controllable degrees of freedom of which are less than the dimensions of the configuration space [6] and is shown in Figure 2.4. The instantaneous curvature of the tip motion is alterable by the offset (δ) between the segments as compared to a car-like robot, in which the turning is dependent on the steering angle (θ). An Electro-Magnetic EM sensor (5 DOF) is fixed at the tip of the probe, to get feedback about the position and orientation of the probe at each instant of insertion. This sensor helps in tracking and localization of the flexible probe during insertion, and guides the probe in reaching the target accurately, while avoiding obstacles. The mechanical constraints of the probe which are to be considered by the path planning approach are the thickness (i.e. the outer diameter) of the probe, the minimum radius of curvature that is achievable by the maximum offset between segments and the maximum limits for the derivative of curvature.

9

2.1 STING: A Soft-Tissue Intervention and Neurosurgical Guide

(a) Reciprocal motion and the resistive forces

(b) Following a pre-defined path

Figure 2.3: (a) Representation of the reciprocal motion and the resistive forces acting on the bevel tip of the probe penetrating the gelatine [4], (b) Example showing flexible probe following a pre-defined path in tissue phantom (i.e. gelatine)

Figure 2.4: Notation comparison between the flexible probe (left) and the conventional bicycle model (right) [6]

2.1.3

System Architecture of the Flexible probe

The overall system architecture of the flexible probe is represented by the block diagram shown in Figure 2.5 [4]. The system is split into two parts, namely, Path Planning block and Path Following block. • Path Planning Block: This thesis work is focused on the Path Planning block. This block is the offline part of the system and takes the pre-operative diagnostic images (MRI as the input images showing the location of the deep brain target) and physical constraints imposed by the flexible probe (minimum radius of curvature) as its input and gives path with minimum risk as its output, for a specific intervention based on brain risk-map. A suitable path planning algorithm is applied within this block to generate optimal path with minimum risk. • Path Following Block: Under the Path Following block, motion signals required to actuate the flexible probe are generated by the low level controller, based on minimum-


10

Figure 2.5: Block Diagram of the system architecture. It is split into two main blocks, namely, path planning and path following, where path planning is the problem under consideration in this thesis.

risk path obtained by path planning block. An electromagnetic position sensor, embedded at the tip of the flexible probe, provides the position feedback to control probe motion, thus, resulting in path following.

2.2

Path Planning Algorithms Overview

The robot motion planning is the process of computing collision free path between the start and target configuration for the robot among obstacles. Path planning deals with the designing of only geometric (kinematical) specifications of the positions and orientation of the robot, where, Trajectory planning includes the designing of the linear and angular velocities as well. The focus in this research work is on path planning approaches [13] [14]. The environmental model of the problem under consideration is static, that means that all information about obstacles is known a priori. A detailed overview of path planning algorithms is presented by N. Sariff [15].

Configuration Space Defining the configuration space (C-space) for a robot is the primary task in any motion planning problem [13] [14]. A configuration describes the pose of the robot, and the C-space is the set of all possible configurations. Arbitrary representation of C-space for a motion planning problem is shown in 2.6. The basic task of any motion planning problem is to find a path from qI to qG in Cf ree , which is the set of configurations that avoids collision with obstacles. Cobs defines the obstacle space and the entire blob shown in Figure 2.6 is represented by C = Cf ree ∪ Cobs [14]. Several motion planning algorithms exists in literature, these algorithms are categorized into two main branches: the classical path planning methods and sampling based path planning methods, and are discussed below:

11

2.2 Path Planning Algorithms Overview

Figure 2.6: The basic motion planning problem is conceptually very simple using C-space ideas. The task is to find a path from qI to qG in Cf ree . The entire blob represents C = Cf ree ∪Cobs [14]

2.2.1

Deterministic methods for Path Planning

Though deterministic methods for path planning are not focus of this research work, but a brief review is presented, merely for the purpose of introducing them to the reader. These methods are sub divided into four main categories [13] [16], which are highlighted below: Potential Field Methods Potential field methods were first described by O. Khatib [17], for online collision avoidance for a robot with proximity sensors. This approach reformulate the motion planning problem as a numerical one. This method makes use of a force field with parameters for the robot, the goal and obstacles. The robot and obstacles are parameterized as positive charges and the goal as a negative charge [16]. The main drawback of this method is that even in the presence of a valid solution, the algorithm fall into a local minima and return no return. Roadmap Methods A map is a data structure used to plan subsequent paths more quickly. The data structure tries to capture the connectivity and features of the configuration space of the robot. Using the map, a planner can find a path between any two configurations by first finding a collision free path from one of the configurations to the roadmap and likewise from the roadmap to the destination configuration. The complexity of this method is limited to simple low dimensional problems. Some of the commonly used roadmap methods are Visibility Graphs, Voronoi diagrams, maximum clearance graphs [13].


12

Cell Decomposition A cell decomposition method represents a free space by the union of simple regions called cells [13]. Two cells are adjacent if they share a common boundary, thus, an adjacency graph shows the adjacency relationship of the cells, where a node corresponds to a cell and an edge connects nodes of adjacent cells. Path planning with a cell decomposition is done in two steps: First, the planner determines the cells that contains the start and goal. Then, the planner searches for a path within the adjacency graph. Two main types of cell decomposition are: Trapezoidal decomposition and Morse decomposition [13] [16]. Grid Search based Methods Grid-based approaches overlay a grid on configuration space, and assume each configuration is identified with a grid point [15]. At each grid point, the robot is allowed to move to the adjacent grid points as long as the line between them is completely contained within Cf ree . This discretizes the set of actions, and search algorithms, like A* or D* are then used to find a path from the start to the goal. These approaches require setting a grid resolution. The search is faster with coarser grids, but the algorithm will fail to find paths through narrow portions of Cf ree . Furthermore, the number of points on the grid grows exponentially in the configuration space dimension, which make them inappropriate for high-dimensional problems.

2.2.2

Sampling based Path Planning Algorithms

Probabilistic path planning methods, also known as Sampling based methods are an important category of algorithms, for solving motion planning problems in Robotics [13] [14]. These algorithms employ a variety of strategies for generating samples (collision-free configurations of the robot) and for connecting the samples with paths to obtain solutions to path-planning problems. These methods can further be categorize as multiple query planners, such as Probablistic Roadmaps, and single query planners, such as Expansive Spaces Trees (EST) and Rapidly-exploring Random Tree (RRT) [13] [18]. These methods are briefly described below: Probablistic Roadmaps (PRM) The Probabilistic RoadMap (PRM) planner is a motion planning algorithm, which uses random sampling of the configuration space for determining a collision free path between the start and goal configuration [19] [20]. The PRM divides planning into two following phases: 1. Construction Phase: A roadmap (graph) is built for approximating the motions that can be made in the environment. First, a random configuration is created. Then, it is connected to some neighbours, typically either the k-nearest neighbours or all neighbours

13

2.2 Path Planning Algorithms Overview

less than some predetermined distance. Configurations and connections are added to the graph until the roadmap is dense enough. 2. Query Phase: The start and goal configurations are connected to the graph, and the path is obtained by a Dijkstra’s shortest path query. Given certain relatively weak conditions on the shape of the free space, PRM is provably probabilistically complete, meaning that as the number of sampled points increases without bound, the probability that the algorithm fails to find a path if at least one path exist approaches zero [19]. The rate of convergence depends on certain visibility properties of the free space, where visibility is determined by the local planner. Several extensions of PRM, such as fuzzy PRM [21], Lazy PRM [22], to name a few, have been developed in past few years. Rapidly-exploring Random Tree (RRT) The Rapidly-exploring Random Tree (RRT) is a planning algorithm developed to quickly search high-dimensional spaces that have both algebraic constraints (arising from obstacles) and differential constraints (arising from nonholonomy and dynamics) [23] [24]. It was proposed by LaValle [23] [24] in 1998, since then it proved to be highly effective and in particular, it is able to solve three main classes of problems, namely, Holonomic, Nonholonomic and Kinodynamic motion planning. Several variants of RRT, such as OBRRT [25], RRT-blossom [26], RG-RRT [27], to name a few, have been develop in past few years. The key idea is to bias the exploration toward the state space, and incrementally pull the search towards them. They consist of a tree data-structure of samples in the space, and an algorithm to create the tree in a way that provides good coverage. The basic RRT algorithm is listed in Algorithm 1, while the extension process is shown in Figure 2.7. The tree-construction algorithm is basically a loop that repeats the following operations: 1. Pick a random sample in the search space. 2. Find the nearest neighbour of that sample. 3. Select an action from the neighbour that heads towards the random sample. 4. Create a new sample based on the outcome of the action applied to the neighbour. 5. Add the new sample to the tree, and connect it to the neighbour.


14

Algorithm 1: T ← BUILD RRT(qinit , qgoal ) 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12:

T ← INITIALIZE TREE(qinit , qgoal ) while (T ∩ qgoal = ∅ and k < K) do qrand ← RANDOM FREE STATE() qnear ← NEAREST STATE(qrand , T ) if NEW STATE(qrand , qnear , qnew , unew ) then T ← ADD NODE(qnew , T ) T ← ADD EDGE(qnear , qnew , unew ) if qnew = qgoal then BREAK end if end if end while

Figure 2.7: RRT Extend operation: a new state qnew is placed along the segment connecting nearest node qnear with the random sample qrand [24] [28]

2.3

Towards Path Planning of Nonholonomic Robots

Path Planning for a nonholonomic robot is a difficult problem, since the planner must check nonholonomic constraints and obstacle avoidance simultaneously. This sections details some of the approaches for path planning of nonholonomic robots, in particular, those capable of constraining the curvature, i.e. the steering angle and giving continuous and smooth paths. These approaches are discussed, starting from the most simplest and commonly used approaches to the most recently developed. The methods which are adapted for path planning of flexible probe are presented in detail.

2.3.1

Dubins’ Curves and its Variants

One of the simplest methods for solving path planning for a car-like robot made use of sequence of Dubins’ curves [29] [30]. The properties and functionality of Dubins’ curve is briefly described here. The path formed by Dubins’ curve is made up of circular arcs connected by tangential line

15

2.3 Towards Path Planning of Nonholonomic Robots

segments. These paths are the shortest ones for car-like robots but the main drawback of these paths is that their curvature is not continuous. Accordingly, a robot following such a path has to stop at each curvature discontinuity in order to reorient its steering. These paths are bounded by maximum steering angles, hence by minimum turning radius constraint. The three motion primitives of Dubins’ are S, L and R. The S primitive drives the car straight ahead. The L and R primitives turn as sharply as possible to the left and right, respectively. Using these symbols, each possible kind of shortest path can be designated as a sequence of three symbols that corresponds to the order in which the primitives are applied. Such sequence is called word. It is discussed in [14], that there is no need to have two consecutive primitives of the same kind because they can be merged into one. Under this observation, ten possible words of length three are possible. Dubins showed that only these six words are possibly optimal [14]: {LRL, RLR, LSL, LSR, RSL, RSR}

(2.1)

The shortest path between any two configurations can always be characterized by one of these words. These are called the Dubins curves. To be more precise, the duration of each primitive should also be specified. For L or R, the subscript α, β or γ denotes the total amount of rotation that accumulates during the application of the primitive. For S, the subscript d denote the total distance travelled. Using such subscripts, the Dubins curves can be more precisely characterized as [14]: {Lα Rβ Lγ , Rα Lβ Rγ , Lalpha Sd Lγ , Lα Sd Rγ , Rα Sd Lγ , Rα Sd Rγ }

(2.2)

in which α, γ ∈ [0, 2π), β ∈ (π, 2π), and d ≥ 0. Figure 2.8 illustrates dubins’ curve trajectories for two words [14]. This idea is extended further, for generating continuous curvature paths, thus giving rise to variants of Dubins’ curve [31] [32]. These dubins’ derived paths are composed of either line segments, a circular arc of maximum curvature, or a clothoid arc. They are called SSC-paths (for Simple Continuous Curvature paths). The SSC-paths are generated between two configurations to solve local path planning problem. This local path planner is then embedded in a global path planning scheme, namely the Probabilistic Path Planner [31] [13], to obtain collision free paths with continuous curvature. Alterovitz, R. [33] in his recent work, used variant of Dubins’ curves, for generating constant curvature paths for bevel tip steerable needles.

2.3.2

Gradient Method for Path Planning

Gradient based nonlinear optimization methods are useful for generating optimal paths for nonholonomic robots. [13]. Two main features of gradient based methods [13] are:


16

Figure 2.8: Dubins curve trajectory for two words are shown in W = R2 [14] 1. It is very general. The motion planning problems for many nonholonomic robots can be encoded as nonlinear programs. 2. The ability to minimize an objective function may result in motions that are efficient in some way. The main drawback of gradient based methods [13] are: 1. A good initial guess must be provided to the solver, as the solver will find only a locally optimal solution. 2. Numerical difficulties, singularities, and nonconvexity may prevent the solver from converging to a solution. Nagy and Kelly [34] proposed a gradient based method in order to get continuous curvature path. The method was based on modelling curvature as a cubic polynomial, in a free configuration space with no obstacles in the environment. Such type of gradient based approach is adopted for generating continuous curvature paths for the neurosurgical flexible probe under consideration, therefore, this method is presented in detail in this section. The gradient based approach for obtaining continuous curvature path [34] assumes that the start posture x0 = [x0 , y0 , θ0 , κ0 ]T and target posture xT = [xT , yT , θT , κT ]T are given. A continuous path connecting start posture with target posture is designed by modelling curvature κ as a cubic polynomial. This method models trajectories as a function of distance rather than time. Nagy and Kelly [34] expressed the state equations for robot posture as: x˙ = V (t) cos(θ(t)) θ˙ = κ(t)V (t)

y˙ = V (t) cos(θ(t)) κ˙ = α/L ˙

(2.3)

17


where V is linear velocity, α the angular velocity, t time, and L the wheel base. The velocity V does not affect trajectory shape, hence it can be ignored. Thus, specification of trajectories can be describe in terms of distance rather than time.

Modelling Curvature as Cubic Polynomial The path describes by cubic curvature polynomial are continuous in curvature [34], thus can be used in steering control. The curvature is modelled as cubic polynomial function of arc length s, and the solution to state equations is given by [34]: κ(s) = κ0 + as + bs2 + cs3 Z s as2 bs3 cs4 θ(s) = θ0 + κ(˜ s)d˜ s = θ0 + κ0 s + + + 2 3 4 Z0 s x(s) = x0 + cos(θ(˜ s))d˜ s Z 0s y(s) = y0 + sin(θ(˜ s))d˜ s

(2.4)

0

where, (a, b, c) are the coefficient of cubic polynomial and s is the arc length. The problem is treated as that of inverse kinematics. Given the start and end posture of nonholonomic robot, the co-efficient of the cubic polynomial and arc length, (known as parameter vector, p = [a, b, c, s]T ), of trajectory can be solved by iterative finite approximation of the Jacobian. These cubic polynomials impose higher order constraints, ensuring smoothness in steering angle acceleration.

Transformation to Local Reference Frame Let x0 = [x0 , y0 , θ0 , κ0 ]T denote the start posture and xT = [xT , yT , θT , κT ]T denote the target posture. The start posture is taken as the reference frame; Hence a transformation is applied to start and target posture in order to express them in local reference frame. Thus, in local reference frame, start and target postures are given by: 

x(0)





0



    y(0)  0       θ(0)  =  0      κ(0) κ0

 ,

x(sT )





L

xT



    y(sT )  L yT   =   θ(s )   L θ   T   T κ(sT ) κT

where, the superscript L denotes local reference frame RL .

(2.5)


18

Table 2.1: Termination Conditions for Gradient Method [34] Condition Allowable cross-track error Allowable in-line error Allowable heading error Allowable curvature error

Value 0.001 m 0.001 m 0.1 rad 0.005 1/m

Initialization of Parameters The parameter vector p must be initialized properly, else the algorithm might not converge. Nagy, B. [34] initialized the parameters by setting c to zero, while using a heuristic to compute s. This heuristic is based on an approximation of the observed average relationship between s and the total change in heading between start and target postures over a large sample set. Then, θ(s) and κ(s) are solved simultaneously to give initial guess for a and b. The initialization equations are given by: d=

q

s = d(

R 2 2 (xR T ) + (yT ) , ∆θ = |θf − θ0 |

2 ∆θ2 + 1) + ∆θ , c = 0 5 5 (2.6)

2κT 4κ0 6 a=− − − 2 (θT − θ0 ) s s s b=

3(κT + κ0 ) 6(θT − θ0 ) − s2 s3

Termination Condition for Cubic Curvature Case The trajectory is computed by iteratively updating parameter vector p and the converges of algorithm is guaranteed only if the termination conditions are satisfied. The termination condition used by Nagy, B. [34] are listed in Table 2.1: Linearisation of State Equation Solution As stated above, parameter vector is given by p = [a, b, c, s]T , while state vector can be given by x = [x, y, θ, κ]T . Eq. (2.4) are treated as a set of four non-linear equations, thus can be rewritten as: x = f1 (a, b, c, s)

,

y = f2 (a, b, c, s)

θ = f3 (a, b, c, s)

,

κ = f4 (a, b, c, s)

(2.7)

19


The four degrees of freedom in the cubic polynomial equations p = [a, b, c, s]T are enough to generate four degrees of freedom in the state space x = [x, y, θ, κ]T . The equations of 2.7 can be written in vector form as [34]: x = f (p)

(2.8)

This non-linear system can be linearised using: ∆x = [

∂ f ]∆p ∂p

(2.9)

Due to sufficient degrees of freedom to control the entire endpoint state, the Jacobian has sufficient rank and is invertable. Thus this can be solved iteratively for parameters converges using: ∆p = [

∂ −1 f ] ∆x ∂p

(2.10)

Inverse Solution for Continuous Curvature Trajectory The problem of finding the parameters that define a valid acceptable trajectory between two given posture is termed as inverse solution. On the other hand, a forward solution gives the posture at any point along the curve, for the given start posture, parameters and length of the curve. In order to get the inverse solution, the two postures are first converted into single relative posture to make the algorithm easier to implement and more generic. Then starting with the initial guess of p as proposed in eq. (2.6), the forward solution is calculated using eq. (2.4) to get the end point x0 for the proposed trajectory. The integrals are computed using trapezoidal method of numerical integration. A finite difference approximation to the Jacobian is then performed using:

Ji,j =

x0j − fi (pj + h) ∂xi = , h = small ∂pj h

(2.11)

The computed Jacobian is inverted at each step of iterative algorithm. The difference ∆x is calculated as: ∆x = x0 − xT

(2.12)

The difference in parameters ∆p is calculated using eq. (2.10) Finally, the parameters update is computed using: p = p + µ∆p

(2.13)


20

where µ is a gain constant, with typical value ranging from 0.1 − 0.3. The process is performed iteratively until the trajectory p converges to an acceptable trajectory. This approach gave solution to continuous curvature path planning without considering the presence of obstacles in the environment.

2.3.3

Gradient Method with Obstacle Avoidance

The gradient based approach towards continuous curvature path planning detailed in Section 2.3.2 does not take into account presence of obstacles in the configuration space. Thompson and Kagami [35], further modified the approach of Nagy and Kelly [34], by modelling the curvature as 4th order polynomial, thus providing a solution for obstacle avoidance. The key idea is to generate a local continuous curvature trajectory using geometric techniques which can incorporate some knowledge of close obstacles. Modelling Curvature as 4th order Polynomial for Obstacle Avoidance Continuous curvature trajectory planning in an environment containing obstacles is formulated by augmenting the robot posture with a cost term, reflecting the cost of travelling along the current trajectory [35]. An estimation of the polynomial that seeks to reach the target state with an obstacle cost of zero is then performed. This idea is similiar to the work presented by O. Lefebvre [36]. In this case, the 4-tuple describing robot posture as defined in Section 2.3.2 now becomes a 5-tuple [x, y, θ, κ, L], where L is the cost term. Thompson, S. [35] used an inverse distance function for L cost term, which evaluates a given position (x, y) based on the inverse Euclidean distance to an obstacle. Adding a term in state vector requires adding a control point or parameter to the curvature polynomial. Thus, a fourth order term is added to the curvature polynomial, resulting in following equations for forward solution [35]: κ(s) = κ0 + as + bs2 + cs3 + ds4 Z s as2 bs3 cs4 ds5 θ(s) = θ0 + κ(˜ s)d˜ s = θ0 + κ0 s + + + + 2 3 4 5 0

(2.14)

The equations for x(s) and y(s) remains the same as described in eq. (2.4), but now they integrate for θ(s) as defined in eq. (2.14). The new term L, describing the cost imposed by the presence of obstacles is formulated as: Z L(s) =

s

( 0

λ λ + ··· + )d˜ s ν0 νN −1

(2.15)

where λ is a constant describing the repulsive quality of obstacles and νi is the distance

21


between a robot posture and obstacle i. The start and target posture are now described by x0 = [x0 , y0 , θ0 , κ0 , L]T and xT = [xT , yT , θT , κT , L]T respectively. Rest of the formulation and computations for path generation are the same as described in Section 2.3.2.

Initialization and Termination Conditions The fourth order polynomial parameters are given by p = [a, b, c, d, s]T . Initialization of the polynomial curve parameters for p is crucial for the iterative approximation to converge on a correct solution. Hence, the output of a cubic curvature polynomial trajectory generator described in Section 2.3.2 is used to initialise the parameters a, b, c and s [35]. This means that a cubic curvature trajectory generation is first performed, then the parameters obtained for generated trajectory are used to initialize a 4th order polynomial parameter. The term d is simply initialized to 0. The termination conditions are same as detailed in Table 2.1, except an addition check for cost is added, that states that difference between cost computed at target and its Jacobian should be less than 0.005.

Formulation of Cost Function Lobs for Obstacle Avoidance Obstacles are considered in path planner by the addition of cost term L to the state space x = [x, y, θ, κ, L]T . In particular, L(s) contains an integral of the sum of the inverse distance to obstacles as illustrated in eq. (2.15). Obstacles are modelled as point obstacles [35] and the distance of obstacles from a position (x(s), y(s)) is given by Euclidean distance. The integral term of eq. (2.15) can then be stated as a sum over N obstacles: Z L(s) = 0

−1 sN X

(p

i=0

λ (x(˜ s) − xi )2 + (y(˜ s) − yi )2

)d˜ s

(2.16)

To assure the convergence on an acceptable solution, S. Thompson [35] rewrote the cost function to produce a minimum of zero for all acceptable trajectories. Thus, the acceptable trajectories are those which do not pass within a clearance distance Dc of any obstacles. The cost function thus becomes: Z Lobs =

−1 s N X

(

0

(

i=0

λ Nλ ))d˜ s−s D(s)i ) Dc

(2.17)

noindentwhere the value of D(s)i , which is the distance of the robot along the trajectory at


22

length s to an obstacle i, is given by following expression: p D(s)i =  (x(s) − xi )2 + (y(s) − yi )2 D(s) if D(s)i < Dc i D(s)i = D if D(s) ≥ D c

i

(2.18)

c

This gradient based method thus provided a solution towards continuous curvature path planning with obstacle avoidance. Note that only point obstacles are considered by S. Thompson [35], thus extending this approach to a configuration space containing irregular shaped obstacles is an open question and will be addressed in next chapter.

2.3.4

Reachability Guided Rapidly Exploring Random Tree (RG-RRT)

Rapidly-exploring Random Tree (RRT) is a single query sampling based approach towards path planning [23] [24], which is detailed in Section 2.2.2. A. Shkolnik [27] proposed a variant of RRT, naming it Reachiablity Guided Rapidly-exploring Random Tree (RG-RRT). Instead of exploring the whole configuration space, this variant search only in the reachable space of nonholonomic robot, thus applying constraints of minimum radius of curvature on it. Jijie Xu [37] [9] used RRT based planner to find paths for a bevel-tip flexible steerable needles in three dimensions. S.Patel [8] showed that RG-RRT speeds up the search process of path planning algorithm for steerable needles, which follows a nonholonomic motion and are constraint by minimum radius of curvature. The RG-RRT based path planning algorithm [8] connect two nodes using circular arcs while respecting minimum radius of curvature constraint and avoiding obstacles at the same time. This approach is adapted for generating paths in two dimensions(2D) for flexible probe [38], which is constraint by maximum curvature bound. Therefore, in this section, RG-RRT is presented in detail. Given an start pose qinit = (xI , yI , θI ) and a desired target position Qgoal = (xT , yT ), the objective is to determine a feasible path such that the nonholonomic robot reaches the desired target position from start configuration while avoiding obstacles and staying within a radius of curvature bound, thus computing a geometric trajectory using RRT-based planner.

RG-RRT based Path Planner Algorithm The key feature of RG-RRT based planner is that instead of exploring random trees in whole configuration space, it only do search and exploring in the space which is reachable by the flexible probe under nonholonomic constraints [27]. The algorithm for generating RG-RRT based planner is outlined in Algorithm 2 and is summarized in terms of points below:

23


• The algorithm takes start pose qinit and a goal position Qgoal as the input and initializes the tree T with qinit . • A random point prand is selected from the configuration space based on some goal biasing, using RRT GoalBias. This function actually replaces the RANDOM STATE function of basic RRT algorithm. Qreach is then set to zero. • The random point is checked with respect to all qi for nonholonomic constraint using REACHABLE in Algorithm 2. • The process is repeated until atleast one reachable configuration is achieved, else the algorithm keeps on sampling new random points. • The reachable random point qrand is then checked for nearest neighbour in Qreach set. The nearest neighbour is addressed as qnear . • SOLVE PARAMETERS generates a local path that connects qnear with prand node. The path is composed of circular arcs in the given case. It then returns the qnew and parameters pnew that defines the path. • Obstacle avoidance is checked by VALID EDGE for this local path and if it is in a clear distance from the obstacle, the respective Node and Edge are added to the Tree T . • The planner incrementally builds a set T of inter-connected configurations that form a tree-like structure over the state space of the flexible probe, while respecting the nonholonomic constraints of the probe and avoiding obstacles in the environment. • This process is repeated until either the tree T connects the start pose and the goal position, or maximum number of iterations is exceeded (k = K), in the later case the algorithm reports that a solution cannot be found. • A path connecting qinit to qgoal ∈ Qgoal can then be found by back tracing the tree from the goal to the root. The RRT-based path planning algorithm, combined with reachability guided heuristic [27] proved to be fast and more efficient than basic RRT-based path planner. This is mainly because of following two features: • Goal Bias Strategy: Basic-RRT based planner samples random point in configuration space. There is no bias towards the goal, which makes the convergence process relatively slow. Further, instead of getting the goal position accurately, a position lying within a goal zone it achieved because of random sampling of configuration space. Therefore, a Goal Bias


24

Algorithm 2: T ← BUILD RG RRT(qinit , Qgoal ) 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12: 13: 14: 15: 16: 17: 18:

T ← INITIALIZE TREE(qinit ) while (T ∩ Qgoal = ∅ and k < K) do repeat prand ← RRT GoalBias() Qreach = ∅ for all qi ∈ T do if REACHABLE(prand , qi ) then Qreach ← Qreach ∪ qi end if end for until Qreach = ∅ qnear ← NEAREST NEIGHBOUR(prand , Qreach ) (qnew ) ← SOLVE PARAMETERS(qnear , prand , T ) if VALID EDGE(qnear , qnew ) then T ← ADD NODE(qnew ) T ← ADD EDGE(qnear , qnew ) end if end while

strategy is introduced which ensures relatively fast convergence along with accomplishing the exact goal position. The RRT GoalBias in line 4 of Algorithm 2 replaces the RANDOM STATE function of basic RRT planner, which tosses a biased coin to determine what should be returned. If the coin toss yields heads, then goal position (xT , yT ) is returned; otherwise, a random state is returned. Even with a small probability of returning heads (such as 0.05), RRT GoalBias usually converges to the goal much faster than basic RRT. If too much bias is introduced, the planner begins to behave like a randomized potential field planner that is trapped in a local minimum. • Reachability Guided Criterion: The flexible probe (STING) is constraint by maximum radius of curvature, which is used to develop the reachability criterion for the algorithm The REACHABLE function in line 7 of Algorithm 2 is formed to check the reachability criterion. For the probe tip configuration which is given by qnear , a random point prand = (px , py ) ∈ R2 is sampled in the configuration space. These points are considered to be in local reference frame RL (aligned to qn ear) instead of global reference frame RG which is aligned at corner of configuration space(image). Let Rmin denotes the minimum radius constraint of the flexible probe. As shown in Figure 2.9(a) , the two circles C1 and C2 with Rmin defines the non reachable space. A random point prand is said to be reachable, if it lies outside these circle. Otherwise, it is not reachable. Thus, the reachability condition for R2

25


space is given by:

Centre of C1 = (xC1 , yC1 ) Centre of C2 = (xC2 , yC2 ) prand = (px , py ) is reachable if:

2 (px − xC1 )2 + (py − yC1 )2 ≥ Rmin 2 (px − xC2 )2 + (py − yC2 )2 ≥ Rmin

Thus, the reachability criterion is expressed as: px ≥

q

2Rmin |py | − p2y

(2.19)

Similarly, as discussed in [8], for a random point prand = (px , py , pz ) ∈ R3 in 3D configuration space, the flexible probe tip is constrained to be inside the volume of a region as shown in Figure 2.9(b) and is defined in local reference frame as:

(a) The reachable set is constrained to be inside the (b) The reachable set is constrained to be inside the area of the region defined locally by eq. 2.19 (shown by volume of the region defined locally by eq. 2.20 (shown the green circle). Point p = (px , py ) is reachable by the colored mesh). Point p = (px , py , pz ) is reachable

Figure 2.9: Reachability criterion in 2D and 3D configuration space

r pz ≥

2Rmin

q

p2x + p2y − (p2x + p2y )

(2.20)


26

Thus, this helps to improve the efficiency of the path planner, by discarding the random points which does not lie within the reachable space of flexible probe.

Geometric Construction of Circular Arc Path The constructed path joining goal position to the start pose of flexible probe in 2D configuration space is defined as a set of circular arcs {C1 , C2 , C3 , ...CN }, where each circular arc Ci is parameterized as a triplet (li , ri , φi ), in which li is the arc length, ri the arc radius and i is the change in orientation of the probe tip. The SOLVE PARAMETERS function computes the circular arc that joins the probe tip configuration qnear = (qx , qy , θ) with the random point pr and = (px , py ), at a particular instant. Note that both qnear and pr and are defined in global reference frame RG (i-e reference frame on image). In order to calculate circular arc between them, first a transformation from global to local frame is performed. Local frame is defined at the probe tip and the transformation matrix is given by: " L

L

TG =

RG

qnear

0

1



#

cos(θ)

 = sin(θ)

−sin(θ) qx cos(θ)

0

0



 qy 

(2.21)

1

This allows the transformation of prand into local reference frame by using: " L

prand =

L

px

L

py

#

  px L −1   = ( TG ) py  1

(2.22)

The geometry for constructing a circular arc is shown in 2.10. The geometric solution for C : {l, r, φ} in 2D is obtained using the following relations [38]: L px ) β = sin−1 ( d d r= sinβ sinφ

,

φ = π − 2β (2.23)

,

l = rφ

Since the reachability condition is already applied, the radius r of circular arc is already constrained to be greater than Rmin of the probe. The orientation θ from qnear is combined with φ to give the new orientation θnew of the probe. Thus, the new configuration of the probe in global reference frame is given by qnew = (px , py , θnear , where (px , py ) is the random point.

27


Figure 2.10: Geometry of constructing circular arc

Collision Checking An important component of RG-RRT is to check whether the circular arc connecting the two configurations qnear and qnew is valid, means it is collision free with respect to obstacles in the environment. The VALID EDGE function in Algorithm 2 performs this check. The circular arc is discretize into points and each point is checked against the respective pixel in the image (configuration space). Note that in configuration space, a pixel value 255 represents an obstacle, while a pixel value 0 represents a free space. If the circular arc comes out to be free from collision, the respective Node and Edge is added to the Tree T . The method proved to be useful in generating curvilinear paths, but does not give smooth paths. Hence, modifcation of this approach is required in order to adapt it for flexible probe.

2.3.5

RRT with Continuous-Curvature Path-Smoothing Algorithm

Nonholonomic motion planning in an obstacle-strewed environment is a difficult problem because the planner must simultaneously consider collision avoidance and nonholonomic constraints. Typical approaches involve generating a piecewise linear obstacle-free path using techniques such as A*, Voronoi diagrams, and probabilistic roadmaps [41] [42] [43] and then applying a secondary-smoothing algorithm over this linear path in order to generate a continuous path for the vehicle to follow. K. Yang [39], proposed an efficient and analtyical continuous-curvature path-smoothing algorithm, that fits an ordered sequence of waypoints generated by an obstacle-avoidance path planner. The algorithm is based upon parametric cubic Bézier curves and requires only the maximum curvature to be defined. K. Yang [39] used this type of approach to generate smooth paths of unmanned aerial vehicles(UAV) in 3D environment [39] [40] [44]. The approach gives G1 and G2 Continuous Cubic Bézier Spiral (G1CBS and G2CBS, respectively) path smoothing.


28

(a) RRT path planner generates a (b) Most extraneous nodes are elim- (c) G2CBS path-smoothing algo3-D collision-free path among obsta- inated by the path-pruning algorithm generates a smooth path satcles rithm isfying the upper bounded curvature constraint

Figure 2.11: Path planning using RRT algorithm and path smoothing using G2CBS algorithm [39] [40]

The approach first make use of the basic RRT planner [23] to find the path. Then, a path prunning algorithm is applied to remove most extraneous nodes from the path. Finally a G1CBS or G2CBS path smoothing is applied, that makes use of cubic Bezier curve [45], and iterative tuning to get bounded continuous curvature path. This is shown in Figure 2.11. Although the approach gives continuous curvature paths, but the computational complexity is relatively high because of path pruning algorithm and iterative tuning of curvature bound. A unique approach is developed for flexible probe path planning, by taking the idea of using Bézier curve for path smoothing. This newly developed approach is presented in Section 3.4.

2.4

Discussion

The strengths and weaknesses of the methods presented in Section 2.3 for nonholonomic robots are summarized in Table 2.2, highlighting the properties under consideration for the path planning of the flexible probe. The key features required for the path planning of the neurosurgical flexible probe are the continuity and boundedness of the curvature and smoothness of the generated path. Though Dubins’ curve [14] [30] is the simplest solution for solving the nonholonomic robot path planning problem but it does not satisfy the continuous curvature constraint required for the path planning of the flexible probe. The variants of the Dubins’ curve [32] [33] solved the problem of continuous curvature but are unable to address the required smoothness property. Gradient based methods [34] [35] seems to solve the problem of the continuous and smooth curvature. Further, RG-RRT path planning method [8] [27] helps in efficient exploring of the configuration space. Although RG-RRT does not give continuous curvature path, but it can be modified to impose the constraints required by the flexible probe. Thus, gradient based

29

2.4 Discussion

Table 2.2: Summary of some Nonholnomic Path Planning Methods

Method Dubins’ Curve [14] [30]

Dubins’ Curve Variants [32] [33]

Gradient based Methods [34] [35]

RG-RRT [27]

[8]

RRT with smoothing approach [39] [40]

Strengths Simplest approach

Weaknesses Discontinuity in curvature

No bound on curvature Simple with low computational cost

Smoothness is not guaranteed and bounding curvature is not easy

Continuous curvature path Geometric approach and solution is deterministic

Computationally expensive

Curvature and its derivative are continuous, hence path is smooth Fast search process as look only in reachable space

Algorithm might fall into local minima, resulting in no solution Curvature derivative is discontinuous, hence path is not smooth

Curvature is continuous and bounded Curvature and its derivative are continuous and bounded

Computationally expensive

method and RG-RRT are adapted for the path planning of the neurosurgical flexible probe and detailed analysis of these methods are presented in Chapter 3. Further, RRT with smoothing approach [39] [40] is also taken into consideration for the smooth path planning of the flexible probe.

Chapter 3

Methods This chapter details some of the existing theories and methods that are applied for solving the path planning problem of the neurosurgical flexible probe robot namely STING(soft-tissue intervention and neurosurgical guide) under its mechanical constraint. The improvements applied to the existing methods in order to adapt them for STING path planning is discussed in detail. A simplified configuration space is used in this chapter to demonstrate the path planning methods, while the results incorporating brain risk-map are presented in next chapter. Further, based on the detailed study of existing nonholonomic system path planning methods presented in Chapter 2, a new approach towards smooth path planning is developed. Section 3.1 presents a gradient based approach towards continuous curvature path planning of the flexible probe, while a sampling based approach is detailed in section3.2 . Further, a locally smooth path planner is designed in section 3.3 using approaches discussed in section 3.1 and 3.2. A new and effective solution for smooth path planning under nonholonomic constraint is developed in section 3.4, Finally a path optimization approach is formulated in section 3.5, which integrates brain risk-map values with path planning approaches, to give most feasible path.

3.1

Gradient based method for Continuous Curvature Path Planning

Planning path for a nonholonomic robot under constraint environment is a non-trivial problem to solve. The neurosurgical flexible probe under consideration is modelled as a nonholonomic car-like vehicle. The task of generating continuous curvature path is performed by using gradient based method as described in detail in Section 2.3.2 and 2.3.3. For better understanding of 30

31

3.1 Gradient based method for Continuous Curvature Path Planning

Figure 3.1: Flow diagram for Cubic Curvature Path Planner

gradient based approach, the flow diagram of cubic curvature path planning algorithm is shown in Figure 3.1. First, a cubic curvature path is generated, then a obstacle collision check is performed. If the cubic path is obstructed by obstacle, then 4th order path is generated to avoid obstacles. Gradient based path planner ensures continuous paths by modelling the flexible probe posture as a 4-tuple (x, y, θ, κ) namely the position, orientation and curvature of the flexible probe. However, there are certain issues, which needs to be addressed during the adaptation of B. Nagy [34] and S. Thompson [35] approach. B. Nagy [34] provided a envelope of target posture values, in local reference frame, for which this approach gave results. This experimentally found envelope was: −1.0m < xT < 1.0m 1.0m < yT < 5.0m

, ,

4π 4π < θT < 5 5 1 1 −0.1 < κT < 0.1 m m −

(3.1)

Clearly, in case of flexible probe path planning, the configuration space is a 2D image, which suggests that the target position will definitely be greater than the envelope defined by B. Nagy [34]. The main reason for defining the envelope was to fix the value of gain constant µ and

Chapter 3: Methods

32

differentiation interval h. Thus in order to adapt this approach for flexible probe, normalization of posture coordinate is performed, which is discussed below:

3.1.1

Normalization of Posture Coordinate

The selection of value of h depends upon the start and target coordinate of posture within the configuration space(which is a 2D image). If the distance between the start and target coordinates of posture is large, h has to be very small (of order 10−8 ). Further, if value of h is not selected appropriately, the Jacobian becomes singular, hence inverse Jacobian failed to exist. In such case, no possible trajectory for the given initial and final posture is found. In order to avoid this problem, the start and target coordinates of posture are normalized with respect to the maximum length of configuration space dimension. It is then found experimentally that the value of h can be fixed to 0.01 in the normalized space. The gradient based method for path planning is thus performed in the normalized space, hence p reflects the trajectory parameters in normalized space. In order to scale these parameters back to the real configuration space, a least square type computation is performed. The normalized sampled coordinates of computed trajectory are obtained by multiplying it with the maximum length of configuration space dimension. Similarly, the length of trajectory is also scaled up to give s in configuration space. Now, let (x0 , x1 , ..., xN −1 ) and (y0 , y1 , ..., yN −1 ) be the coordinates of the computed trajectory with constant step size ∆s. The heading angle (θ0 , θ1 , ..., θN −1 ) is also known for the computed trajectory and is not effected by normalization. Using eq. (2.4) for θ(s), this information can be rewritten for every step change in s as:  θ0 θ(s(0))      θ(s(1))  θ0  = ..   .    .. .   θ(s(N − 1)) θ0 



κ0 s(0) κ0 s(1) .. . κ0 s(N − 1)

s(0)2 2 s(1)2 2 .. .

s(0)3 3 s(1)3 3 .. .

s(N − 1)2 2

s(N − 1)3 3

s(0)4 4 s(1)4 4 .. .



   1    1    a      b 4 s(N − 1) c 4

(3.2)

A least square solution of the above expression then gives the true value of polynomial coefficients a,b and c, thus giving the true trajectory p. Another issue under consideration was the redesigning of cost function of obstacle avoidance, in order to take into account irregular shaped obstacles as well. This problem and its solution is detailed below:

33


3.1.2

Designing Cost Functions for Obstacle Avoidance

The cost function for obstacle avoidance proposed by S. Thompson [35], is given by eq. (2.17) and is discussed in detail in Section 2.3.3. Figure 3.2 shows the path generated in the presence of single point obstacle, with the clearance distance of Dc = 15. Cubic curvature path is shown in red, which is obstructed by an obstacle, while the 4th order obstacle free generated path is shown in blue, for the start and target positions marked with green star. The distance D(s)i ) along this red trajectory is also shown. S. Thompson [35] showed in his work, that this cost function is only effective for few number of point obstacles in workspace. In case of irregular shaped obstacles, every position showing obstacle in workspace can be considered as individual obstacle, even though these positions are connected. Thus, resulting in an increased number of obstacles. Clearly, in such situation the approach becomes inefficient and does not converge to a solution. Further, there is no use in looking at all the obstacles in the environment for developing cost function, since not all obstacles will obstruct the generated trajectory. Thus, a modified cost function is designed, in this thesis work, which proved to be a useful addition for this approach.

Formulation of modified Cost Function Lef f for Obstacle Avoidance To overcome the limitations of Lobs function, a modified L function Lef f is designed by investigating the properties of Lobs . This function behaves in similar way as Lobs , while utilizing comparatively less computational resources. The main idea behind its design is that not all obstacles in a configuration space falls within the generated cubic trajectory. Only a local neighbourhood of pixels along the cubic curvature path is enough to generate an obstacle cost value. The distance Dc in this case, represents the clearance required from the obstacles. A local neighbourhood window is placed on every discrete point of cubic trajectory. The size of this window depends upon Dc and is of (2Dc + 1) × (2Dc + 1). The pixels in this local neighbourhood window are convolved with Gaussian function of same size centred at the particular trajectory point. The result of which is a weighted value that reflects the information of how close or how far an obstacle is. This value is then subtracted from the clearance distance Dc to make the performance similar to Lobs distance. Mathematically, this is given by: D(s)j = Dc − Dc O(xj , yj )

(3.3)

Chapter 3: Methods

34

(a) Cubic curvature path (red) obstructed by an obstacle and 4th order obstacle free path (blue)

(b) Curvature of 4th order path

(c) Derivative of Curvature of 4th order path

(d) Distance D(s) for first instant of red trajectory

Figure 3.2: Method 2: Continuous Curvature Path Planning obtained by using Lobs cost for obstacle avoidance. (a) shows cubic polynomial path in red and 4th order path incorporating obstacle avoidance in green.

where, xj and yj is the ith sampled trajectory point

O(xj , yj ) =

Dc X

Dc X

I(xj + k, yj + l)G(k, l)

(3.4)

k=−Dc l=−Dc

where, I is the image representing configuration space and G is the Gaussian kernel.

When there will be more white pixels(obstacles) in the neighbourhood of I(xj , yj ), the value of O(xj , yj ) will be high, hence D(sj ) will be low. Thus, this function is designed in such a way, that it gives similar repulsive power as that of Lobs functionll, which is shown in Figure 3.2(d). Using this local neighbourhood type metric measure, the Lobs function in eq. (2.17) can be

35


(a) Cubic curvature path (red) obstructed by an obstacle and 4th order obstacle free path (blue)

(b) Curvature

(c) Derivative of Curvature

(d) Distance D(s) for first instant of red trajectory

Figure 3.3: Method 2: Continuous Curvature Path Planning obtained by using Lef f cost for obstacle avoidance. (a) shows cubic polynomial path in red and 4th order path incorporating obstacle avoidance in green.

rewritten in the following way to give Lef f .

Z Lef f =

s

( 0

λ λ )−s D(s)j ) Dc

(3.5)

The formulated Lef f function does not have to take into account all the obstacles in the configuration space. Besides, it works only with obstacles present in local neighbourhood and deforms the trajectory of cubic curvature polynomial into 4th order polynomial to give obstacle free trajectory. Further, since this function is applied using configuration space, the value of Dc is not huge and is of order 1 to 7, thus, convolution operation is not expensive. Figure 3.3 shows the obtained result in a configuration space containing irregular obstacles. The distance measure for red trajectory is also shown, which has a similar response as Lobs distance measure.

Chapter 3: Methods

3.1.3

36

Strength and Weaknesses

The continuous curvature trajectory planner discussed in this section provides a deterministic approach for generating paths with continuous curvature and continuous derivative of curvature, hence smooth paths are generated. The method is useful in generating precise postures and follow precise trajectory. However, the curvature can not be bounded in this method. Further, the posture definition is very important, since if the orientation θ of start and target posture is not defined properly, it will result in either a very long path or no solution. The algorithm is of iterative nature, as a result of which it is time consuming. Further, generating 4th order polynomial for obstacle avoiding is more time consuming then cubic polynomial model. Also, this method is not applicable, if the configuration space is complex, since a 4th order polynomial can not give too much bending.

3.2

Path Planning using Rapidly Exploring Random Trees

Sampling based algorithms such as Rapidly Exploring Random Trees(RRTs) provides an efficient randomized planner [23]. Modified RRT based planner is use for generating path planner under constrained conditions. Sampling based methods are detailed in Section 2.2.2, while an RRT methos using reachability guided heuristic is detailed in Section ??asic RRT is presented in detail in Section 2.3.4. RG-RRT is adapted for path planning of flexible probe. The goal is to guide the tip of flexible probe(STING) to the target position (xT , yT ) in the configuration space while avoiding any obstacles. The thickness of flexible probe is assumed to be equal to two pixels hence the configuration space is developed by dilating obstacle borders by the thickness of flexible probe. Given an initial configuration of flexible probe pose qinit = (xI , yI , θI ) and a desired needle target position Qgoal = (xT , yT ), the objective is to determine a feasible trajectory such that flexible probe reaches the desired target position from initial configuration while avoiding obstacles and staying within a radius of curvature bound, thus computing a geometric trajectory using RRT-based planner.

3.2.1

RG-RRT based Path Planner Algorithm

The key feature of RG-RRT based planner is that instead of exploring random trees in whole configuration space, it only do search and exploring in the space which is reachable by the flexible probe under nonholonomic constraints. The flow diagram in Figure 3.4 summaries the complete algorithm for generating paths using RG-RRT sampling based method, while detail of this algorithm is presented in Section 2.3.4. Figure 3.5 shows the comparsion of basic RRT with RG-RRT. First row shows the path obtained by basic RRT, while second row display the path obtained by RG-RRT. Clearly, in

37

3.2 Path Planning using Rapidly Exploring Random Trees

Figure 3.4: Flow diagram for Reachability Guided Rapidly-exploring Random Tree (RG-RRT)

basic RRT, since the path is without reachability guided criterion and curvature bound, the results are far beyond optimal. But, in case of RG-RRT, the obtained results are near to optimal. The results obtained by applying RG-RRT based path planning approach are shown in Figure 3.6. The path is formed by multiple circular arcs, while setting a maximum curvature bound of κmax = 0.0125. This means that only those circular arcs are selected, which comes under bounded curvature limit. Figure 3.6(b) shows that curvature is discontinuous.

3.2.2


The path obtained by RG-RRT based planner is of probabilistic nature that suggests that on every trial a different path will be obtained. RG-RRT based path planner is successful in generating bounded curvature path and is relatively more effective than Gradient based approach, but, the main drawback of this planner is that since path is made of circular arcs, the curvature is discontinuous. The flexible probe mechanical constraints does not allow it to change curvature drastically. The generated path is clearly not smooth, since derivative of

Chapter 3: Methods

38

Figure 3.5: Comparsion of basic RRT with RG-RRT. First row indicates results obtained by using basic RRT, while second row indicates result obtained by RG-RRT

curvature is discontinuous at nodes connecting two edges(circular arc). Hence there is a room for improvement in this method, therefore, further approaches are investigated.

3.3

Locally Smooth Path Planner

The gradient based path planning approach of Section 3.1 gives path which are smooth in terms of curvature and derivative of curvature. This method is deterministic and has limitations as discussed in Section 3.1.3. Further, if the configuration space comprises complex obstacles, it is nearly impossible to model the path with a single polynomial model. Another limitation is that if the target or start point is close to obstacles, the angle θ needs to be adjusted manually several time to get possible solution. Otherwise no solution exists. But the most relevant limitations is that the curvature cannot be bounded. Further, the RG-RRT based planner discussed in Section 3.2, produces paths with discontinuity in curvature. In order to overcome the limitations of gradient based path planner and RG-RRT based path planner, the idea of Locally Smooth Path Planner is formulated. This new planner for bounded

39

3.3 Locally Smooth Path Planner

(a) Configuration Space

(b) Curvature


Figure 3.6: Method 1: Reachability Guided Rapidly Exploring Random Tree (RG-RRT)

Figure 3.7: RG-RRT with CCT flow

continuous curvature path generation of flexible probe, is formed by with the integration of RG-RRTs with cubic curvature path planner. This generates a sub optimal path, bounded by maximum curvature limit, even if the target or start point is located close to an obstacle. The problem is considered to be similar to the one described in section 3.1. This method assumes that the start posture x0 = [x0 , y0 , θ0 , κ0 ]T and target posture xT = [xT , yT , θT , κT ]T are given. However, the target posture orientation θT is not a fix value in this case, instead a range of target orientation is defined. The range is sub-divided to give a set of target orientations. A fix value of κ is taken throughout the algorithm.

3.3.1

Integration of RG-RRT with Continuous Curvature Path Planner

This approach is formed by integrating RG-RRT as defined in section 3.2 with cubic continuous curvature path planner as discussed in section 3.1, which give a locally smooth path planner. The algorithm works by rapidly exploring the configuration space under reachability guided criterion, but instead of generating circular arc paths between the configuration qnear and random point prand , it makes use of cubic curvature path planner for generating paths. The

Chapter 3: Methods


40

(b) Curvature


Figure 3.8: Method 3: Locally Smooth Path Planner formed by the integration of RG-RRT and Continuous Curvature polynomial path generator

function SOLVE PARAMETERS in RG-RRT Algorithm 2 is replaced with cubic curvature path planner algorithm. The block diagram in Figure 3.7 details this feature of locally smooth path planner. The random point in case of RG-RRT is transformed into random configuration by adding the orientation in it. This orientation is selected randomly from the set of orientations defined during initialization for target. κ is set to a fixed value. Now, for posture qnear = (qx , qy , θ, κ) and qrand = (qrandx , qrandy , θrand , κ) in the new function SOLVE PARAMETERS, the iterative cubic curvature path planner algorithm is applied. This generates a smooth path between the two postures. The generated path is then tested for obstacle avoidance by checking against the respective pixel in configuration space. If it is free from obstacles, the node, edge and path parameters are added to the tree T , else the algorithm moves on to explore further. The computed path is also checked for curvature and derivative of curvature bound, and the local path is discarded if it exceeds this limit. The algorithm runs iteratively until a solution is obtained or the maximum number of iterations are achieved, in the later case no solution is found. The results for locally smooth path planner are shown in Figure 3.8. The curvature is continuous and smooth, while the derivative of curvature is only locally smooth. Further, the generated curvature is bounded by reachability criterion.

3.3.2


The approach has several advantages over RG-RRT and continuous curvature path planner approaches. The generated path is continuous in terms of curvature and locally continuous in terms of derivative of curvature as compared to RG-RRT, in which derivative of curvature is not continuous. The path is bounded with respect to curvature and derivative of curvature. The only drawback is the computation time. This approach is less efficient compared to the

41

3.4 Smooth Path Planning Algorithm

previously discussed algorithms.

3.4

Smooth Path Planning Algorithm

In Section 2.3.5, an analytical approach towards continuous curvature smooth path generation is discussed which is developed by K. Yang [39]. Despite having several advantages, this approach is not efficient, which is mainly due to path prunning algorithm and iterative tunning to get bounded curvature path. Keeping the theory presented in K. Yang work in mind, a new approach towards smooth path planning is developed, which generates bounded, continuous curvature smooth path for flexible probe and is relatively more efficient than all the approaches formulated above. Given the initial configuration qinit = (xI , yI , θI ) of flexible probe and a desired needle target position Qgoal = (xT , yT ), the task is to develop an approach that generates smooth paths in terms of curvature and derivative of curvature. The approach makes use of RG-RRT based planner for path generation and Bézier curve for path smoothing. In the interest of this newly formulated path planner, it is important to detail basic mathematics of Bézier Curve.

3.4.1

B´ ezier Curve Theory and Mathematics

Bézier Curves and B-spline curves are the two most important types of approximating curves widely used in computer Graphics nowadays. Bézier Curves named after Pierre Bézier are the most common representation for free form curves. A Bézier curve [45] is a polynomial curve that approximates its control points. The curves can be a polynomial of any degree. A curve of degree n is controlled by n + 1 control points. The curve interpolates its first and last control points, and the shape is directly influenced by the other points. Construction Bézier curves are easy to control, have a number of useful properties and there are very efficient algorithms for working with them. Bézier curves are constructed [45], such that: • The curve interpolates the first and last control points, with t = 0 and 1, respectively. • The first derivative of the curve at its beginning (end) is determined by vector between the first and second (next too last and last) control points. The derivatives are given by the vectors between these points scaled by the degree of the curve.

Chapter 3: Methods

42

Figure 3.9: Various Bézier segments of degree 2 to 5. The control points are shown with red dots, and the control polygons(line segments connecting the control points) are shown with blank lines. • Higher derivatives at the beginning (end) of the curve depend on the points at the beginning (end) of the curve. The nth derivative depends on the first (last) n + 1 points.

Generalization The Bézier curve of degree n can be generalized is discussed below. Given points P0 , P1 , · · · , Pn , the Bézier curve is defined as:

n X n

(1 − t)n−1 ti Pi i=0 n n = (1 − t)n P0 + (1 − tn−1 )tP1 + · · · + (1 − t)n−1 Pn−1 + tn Pn 1 n−1 B(t) =

i

(3.6)

n is the binomial coefficient and t ∈ [0, 1] i The points Pi are called control points for the Bézier curve. The polygon formed by con-

where,

43

3.4 Smooth Path Planning Algorithm

Figure 3.10: Flow of globally smooth path planner, which involves RG-RRT for path planning and Bézier curve for path smoothing

necting the Bézier points with lines, starting with P0 and finishing with Pn , is called the Bézier polygon (or control polygon). Figure 3.9 shows various Bézier segments of degree 2 to 5. A Bézier curve B(t) defined for four control points P0 , P1 , P2 , P3 is given by: B(t) = (1 − t)3 P0 + 3(1 − t)2 tP1 + 3(1 − t)t2 P2 + t3 P3 , x(t) = (1 − t)3 x0 + 3(1 − t)2 tx1 + 3(1 − t)t2 x2 + t3 x3

t ∈ [0, 1] and

(3.7)

y(t) = (1 − t)3 y0 + 3(1 − t)2 ty1 + 3(1 − t)t2 y2 + t3 y3 A Bézier curve can also be expressed recursively and a Bézier curve of degree n is formed by linear interpolation between two degree (n − 1) Bézier curves. For a Bézier curve BP0 P1 ···Pn , determined by control points P0 , P1 , · · · , Pn , the recursive expression is given by: B(t) = BP0 P1 ···Pn (t) = (1 − t)BP0 P1 ···Pn−1 (t) + tBP1 P2 ···Pn (t)

(3.8)

The curvature κ of a parametric curve( thus for a Bézier curve), in general is defined as [46]: κ=

3.4.2

|x¨ ˙ y − y¨ ˙ x| (x˙ 2 + y˙ 2 )3/2

(3.9)

Globally Smooth Path Planner

A global path planner approach is developed using Bézier curve, for generating smooth paths. This approach is summarized in Figure 3.10. RG-RRT is first used to compute the polygon segments for Bézier curve. The algorithm is same as discussed in Section 3.2. Only the function SOLVE PARAMETERS is replaced with a routine that generates line segments instead of circular arcs. These line segments are then checked for collision with obstacles using function VALID EDGE in Algorithm 2. If the line segment is collision free, its nodes and edges are added to the tree T . In this way, an obstacle free segmented path is obtained from start

Chapter 3: Methods


44

(b) Curvature


Figure 3.11: Method 4: Globally Smooth Path Planner developed using Bézier Curve configuration to target position. Now, the nodes for this path corresponds to control points for Bézier curve, while the line segments forming the path corresponds to Bézier polygon segments. A Bézier curve is then generated using these control points, which gives a smooth path from start to target points in configuration space. Finally, a collision check is performed on this smooth path to check if the generated path is free from obstacles. If a collision with obstacle occurs, the path is discarded, and the algorithm is repeated again until it gets a collision free smooth path. Figure 3.11 shows the path obtained using this method. Note that the curvature is bounded and, curvature and its derivative are both continuous hence giving smooth paths. One of the key condition of this method is to accept only those outputs of RG-RRT, which have more then two nodes (control points), else the generated path would be a straight line which is not acceptable.

3.4.3


This approach proved to be efficient than all other approaches discussed in this chapter. Further, the generated paths are smooth. Since the Bézier polygon is build using reachability guided RRT, the curvature of the generated curve is already limited. Thus, this eliminated the need of using path-prunning algorithm for removing extraneous nodes from RRT generated path, as applied by K. Yang [39], [40], [44]. This approach proved be an easy, effective and efficient solution for generating smooth paths for flexible probe.

3.5

Optimization of Path

The algorithms discussed so far generates paths using binary configuration space,i.e. a space containing no obstacles (pixel value = 0) or obstacles (pixel value =255). But in real, the brain

45

3.5 Optimization of Path

structure can divided into much complex space to give riskmap of brain. A predefined riskmap of coronal section of brain is provided in this research work for testing of developed algorithms. The riskmap assigns risk values to different parts of the brain depending upon the sensitivity of part. Most of the algorithm proposed above are of probabilistic nature and gives sub-optimal paths. Thus every time the algorithm is executed, it gives different path for same start and target position. There is a need to define a cost associated with each generated path, such that minimizing this cost can give optimized path. Y. Kanayama [47] proposed cost functions related to integration of square of acceleration and integration of square of variation of acceleration. Reuben. R. Shamir [48], [49] proposed risk volume and trajectory risk expression to reduce patient risk in image guided neurosurgery. S. Patil [8] proposed an optimization criterion which tries to find the shortest path with maximum clearance from obstacles. Similar work is reported [38], in which risk based criterion is also computed. The task in this research work is that to implement risk based path planning, using the provided risk-map of brain. The main structure of the brain is classified into six labels according to the level of risk associated with the insertion of the probe into each structure. The labels are Accessible, Common, Careful, Warning, Dangerous, Avoid, each of which is assigned a grayscale value ranging from 0to255 respectively. Black denotes an accessible area while white denotes an impenetrable area. Further, in any path planning algorithm, Avoid is defined as obstacle and the algorithm must avoid them. While for other five levels, a cost function is to be defined to give path risk value. Finally, the goal is to find an optimal path that takes into account Risk-map of brain and respects flexible probe constraints, while giving the best compromise between minimum risk and shortest path.

3.5.1

Optimization Criteria

Due to the probabilistic nature of all the algorithm discussed above, except for the gradient based method in Section 3.1, the generated path is sub-optimal and is different at each trial. All the four developed approaches are adapted for the provided risk-map of the brain, to give optimal path. In order to find the optimal path, multiple paths are computed by executing the algorithm several times. In case of gradient-based path planner of Section 3.1, the solution is deterministic, so in order to get multiple paths, a range of orientation of target posture is defined and several paths are computed by changing the orientation of the target posture within this range. Let us say, that one of the above mentioned probabilistic approach is executed multiple times to give five different paths for same start pose and target position. The task is to find

Chapter 3: Methods

46

the optimal path out of these five sub-optimal paths. When selecting an optimal path from a set of feasible planned paths, the goal is to damage the least amount of brain tissue(i-e. shortest path), while at the same time maximizing clearance from obstacles and considering the risk-map of the brain. In order to develop the optimization criteria, following five different cost functions are defined: 1. Cpath defines the length of path from start and target points. 2. Cobstacle defines the distance of closest obstacle from the path. 3. Crisk gives the value obtained by adding the brain Risk-map value of all pixels from which the planned path is crossed. 4. Ca is formed by taking the integration of (square of) acceleration as path cost. This is given by the expression: Ca =

1 s

s

Z

κ2 ds

(3.10)

0

5. Ca0 is formed by taking the integration of square of the variation of acceleration (or jerk) as path cost and is given by the expression: Ca0 =

3.5.2

1 s

Z

s

(κ) ˙ 2 ds

(3.11)

0

Total Optimal Cost Formulation

The individual cost doesn’t have any link among themselves and are all independent of each other. In order to relate each of them, relative normalization is done for individual costs, over n n all five paths. Thus, after normalization, all five cost functions are rewritten as: Cpath , Cobstacle , n Crisk , Can , Can0 , where, the superscript n denotes that all costs are relatively normalized. For the n n n costs Cpath , Cobstacle , Crisk , weighting parameters α, β, γ are defined, such that α + β + γ = 1.

A total cost function for a particular path out of the five computed one is given by: n n n CT otal = αCpath + β(1 − Cobstacle ) + γCrisk

(3.12)

Similarly, total cost CT otal is computed for all the feasible paths. User defines values for α, β, γ, depending upon the requirement. If it is more important to get a shortest path, the value of α is set to one while that of β and γ are set to zero. Using eq. (3.12), total cost for five paths is computed. The path with minimum total cost is selected at the optimal path.

47

3.6 Discussion

3.6

Discussion

Four different methods for the path planning of neurosurgical flexible probe were presented and analysed using a binary configuration space. These developed methods output discretized path, sampled using uniform step defined by the user. This sampled path then become input to the path following block as described in Section 2.1.3, where the path follower block generates the necessary control inputs for the flexible probe motion. The analysed path planning methods have their own strength and weaknesses. The gradient based method gave continuous and smooth paths but was unable to bound the curvature. RGRRT based path planning method gave fast exploring of configuration space and combining it with gradient method, resulted in a locally smooth path planner. Although, the locally smooth path planner solved the problem of bounding the curvature and producing continuous and smooth path, but it was highly inefficient. A complete solution to the path planning problem of the flexible probe is obtained with the development of globally smooth path planner. A path optimization scheme was also developed, which took into account factors, such as minimum damage to the tissue, maximum clearance from obstacles and minimum risk to the patient. Testing of these approaches on provided riskmap of brain is presented in Chapter 4.

Chapter 4

Results and Comparison This chapter details the testing of path planning algorithms developed in Chapter 3 for the neurosurgical flexible probe, on the provided risk-map of the brain. It involves preparing the configuration space for the flexible probe, which is discussed in Section 4.1. Once the configuration space for the provided risk-map of the brain is formed, all four develop algorithms discussed in Chapter 3 are tested on it and an optimized path is selected. This is presented in Section 4.2. A thorough comparison of all four developed approaches is then made and is detailed in Section 4.3. The algorithms are developed on an Intel Core 2 CPU 6300 @ 1.86GHz PC, with 2GB of RAM, using Matlab version 7.9 for programming.

4.1

Brain Risk-Map and Configuration Space

The coronal section of risk-map of brain is provided, which is a grayscale image of size 153x153 (as shown in Figure 4.1(a)). This map classify anatomical structure of brain into six levels of risks. These level of risks are labelled as accessible, common, careful, warning, dangerous and avoid, ranging from black to white gray values respectively. The regions towards the darker gray value are at low risk and are penetrable by the flexible probe. While the ones towards brighter side are at high risk value. Thus, the region labelled ’Avoid’ is considered as ’no-go area’ and must be avoided by the path planning algorithms. First task is to prepare this risk-map image to give the configuration space. To do this, the obstacles (namely, the ’Avoid’ label) are dilated with the thickness of the flexible probe, taken to be of 2 pixels size, while the scale is taken to be 1 pixel = 1 mm. By applying morphological operations, the brain map is converted into a binary image as shown in Figure 4.1(b), which defines the configuration space for the neurosurgical flexible probe. The regions marked in white are the no-go areas, while the one in back are the accessible areas. 48

49

4.2 Testing Path Planner on Brain Risk-map

(a) Coronal section of Risk-map of brain

(b) Configuration Space

Figure 4.1: The coronal section of risk-map of brain, with all six levels of risk labelled. The configuration space for flexible probe is extracted from the Risk-map

4.2

Testing Path Planner on Brain Risk-map

Once the configuration space on brain risk-map is formed, the developed algorithms discussed in Chapter 3 are tested on it. Path planning involves using the developed configuration space, while for path optimization, the original workspace (i.e. the riskmap of the brain) is also used to get risk cost value as discussed in Section 3.5. Results for both path planning and path optimization are discussed below for all four developed approaches.

4.2.1

Results for Gradient based method for Continuous Curvature Path Planning

The gradient based approach giving geometric solution to get continuous curvature path is discussed in Section 3.1. This section presents the results obtained by applying the gradient based approach on provided risk-map of brain. This approach is totally deterministic. For this algorithm, input is the start and target posture and is selected to be x0 = [30, 30, π/4, 0.008]T and xT = [65, 80, θT , 0.008]T respectively, where as detailed in Section 3.1, a posture is defined by [x, y, θ, κ]T . The value for κ at start and target depends upon the user and is selected to be small in this case. Note that for start posture θ is fixed and is a single value normal to the skull surface, but in target posture, it is selected to be in the range(−0.2 to 1.0 rad) of possible orientation. This selection depends upon the user and is incremented with fixed step size (0.2 in this case), to get multiple solutions. The value of µ and h is selected to be 0.2 and 0.01 respectively for this particular case. By varying the target orientation, multiple trajectories are obtained, which are purely deterministic and the result is shown in Figure 4.2(a). All obtained trajectories are color coded in the example and their curvature and derivative of curvature is

Chapter 4: Results and Comparison


50

(b) Curvature


Figure 4.2: Gradient based path planner results showing the effect of varying target orientation θ

(a) Shortest Path (α, β, γ) (1, 0, 0)

=

(b) Path with maximum clearance form obstacle (α, β, γ) = (0, 1, 0)

(c) Path with minimum (α, β, γ) = (0, 0, 1)

risk

Figure 4.3: Results for optimization of path obtained by applying gradient based approach of path planning, which is obtained by varying target orientation value

shown in Figure 4.2(b) and (c). Note that the curvature and derivative of curvature are continuous polynomial of order 3rd and 2nd respectively, for this particular case. The bounded curvature constraint cannot be applied on this method. But as shown in Figure 4.3(b), a path with minimum curvature or with a curvature within the bounded constraint can be selected manually by the user. The multiple paths generated by varying target orientation are then use to get optimal path. In similar way as discussed in previous section, the path with shortest length or maximum clearance from obstacle or minimum value of risk are computed and are shown in Figure 4.3. In terms of obtaining continuous curvature and derivative of curvature for a non holonomic type robot, this sure is a good solution, though it is computationally expensive because of its

51



(b) Curvature


Figure 4.4: Result obtained using RG-RRT based path planner. Random branches are shown in blue, nodes are shown by red dots while the computed final path is shown in green.


=



risk

Figure 4.5: Optimzation of path for RG-RRT based planner obtained by generating multiple paths iterative nature and involvement of integral terms in its mathematics. Though not shown over here, but the algorithm involving 4th order curvature polynomial model for obstacle avoidance is even more expensive in terms of computations.

4.2.2

Results for RG-RRT based planner

For the RG-RRT based path planner, Rmin is selected to be 35 mm, which gives maximum curvature value to be κmax = 1/Rmin = 0.029. For the results shown in Figure 4.4, the inputs to the algorithm are the start pose and target position and are selected to be qinit = (30, 30, π/4) and qgoal = (65, 80). Clearly, the orientation at start(input) is taken to be normal to skull surface. K= 3000 is taken to be the maximum number of iteration for RG-RRT growth, as



52

(b) Curvature


Figure 4.6: Results obtained by applying Locally Smooth Path Planner on coronal section of brain Risk-map

defined in Algorithm 2 in Section 3.2. Figure 4.4(a) shows the search process in configuration space where the final path found is shown in green and tree nodes are shown with red dots. The curvature of path is shown in Figure 4.4(b), which is clearly within the maximum curvature bound. But as the curvature of path is a step type function, the derivative of curvature is discontinuous. In reality, the flexible probe motion cannot have such discontinuities, which is the main drawback of this approach. The RG-RRT based path planner is of probabilistic nature, hence on each trial a different result is obtained. The optimization approach makes use of this probabilistic nature of algorithm and several paths are computed, while the best path in terms of cost is selected among them. As shown in Figure 4.5, RG-RRT is executed five times to give five different paths. Then the optimization approach defined in Section 3.5 is applied. Figure 4.5(a) shows the shortest path, while the path having maximum clearance form obstacle/no go area is shown in Figure 4.5(b). The path with minimum risk is shown in Figure 4.5(c). A compromised path in terms of these three features can also be obtained by selecting different values of α, β, γ such that α+β +γ = 1. This strategy along with reachability guided criterion gives the most optimal path possible for a search space having six levels of risk.

4.2.3

Locally Smooth Path Planner

The locally smooth path planner is detailed in Section 3.3, results for which are discussed in this section when applied on Risk-map of brain. The input to the algorithm is the start posture x0 = [30, 30, π/4, 0.008]T and target posture xT = [65, 80, θT , 0.008]T , while the orientation at target is a range of values instead of one fix value. The RG-RRT based planner is then applied which search for nodes in tree based on reachability criterion. Once found, these nodes are

53



(b) Curvature


(d) Zoomed view of generated path

Figure 4.7: Globally smooth path planner results, where the generated path(in green), control points(red dots) and bézier Polygon segments(blue) are clearly shown in zoomed version

connected by 3rd order geometric solution as discussed above. One of the key addition to this approach is that every time when a geometric solution is evaluated, the value of orientation of new node is selected randomly from range of defined orientation. The result obtained when this method is applied on brain risk-map is shown in Figure 4.6. The path is continuous between two nodes while the derivative of curvature is only locally continuous but this feature is not obvious from the result on brain Risk-map. These features are more clear from example shown in Figure 3.3 of Section 3.1 The significance of this method is not so clear from these results, which is merely because the size of configuration space is small, but it is a useful approach when the search space is large and complex and require continuous paths. One of the main drawback of this approach is that it is highly time consuming, and this time varies on each trial as the algorithm is of probabilistic nature.



=


54


risk

Figure 4.8: Results for optimization of path obtained by applying Globally smooth path planner by generating multiple paths

4.2.4

Result for Globally Smooth Path Planner

Globally smooth path planning approach is developed in Section 3.4. Here, the results are shown when the globally smooth path planner is applied on coronal section of brain risk-map. Like the RG-RRT based planner, this algorithm takes start configuration qinit = (30, 30, π/4) and target position qgoal = (65, 80) as input. The minimum radius of curvature is selected to be Rmin = 35 for the given riskmap of brain. This method computes feasible path by connecting nodes through line segments. Once a possible path is found, each node is then addressed as control points for bézier curve while the line segments joining the control points are termed as Bézier polygon segments. A bézier curve is drawn using this information from start to target point. The obtained result for applying this method on risk-map of the brain is shown in Figure 4.7. The search tree is shown in blue, while the smooth path generated is shown in green and control points are represented by red dots. A zoomed version of the obtained path is also shown in Figure 4.7(d), which clearly shows the obtained smooth path using bézier curve, control points and bézier polygon segments. The path obtained is continuous in terms of curvature and derivative of curvature as shown in Figure 4.7(b) and (c), hence guaranteed smoothness is achieved. Moreover, the solution is bounded by the curvature constraint and the efficiency of this approach is higher then other approaches. For finding the optimal path, the approach discussed for optimization in Section 3.5 is applied. As this approach is of probabilistic nature, five smooth but different paths connecting start point to target point are computed. Using the total cost function CT otal defined in eq. 3.12, the shortest path, the path with maximum avoidance from the obstacle and the path with minimum risk are computed, which are shown in Figure 4.8.

55

4.3 Comparison of Developed Path Planning Methods

Table 4.1: Comparison of Developed Path Planning Methods

Path Planning Methods

Nature of Algorithm Curvature Derivative of Curvature Input Parameters

Gradient Method (section 3.1) Deterministic

(section 3.2) Probabilistic

Locally Smooth (section 3.3) Probabilistic

Globally Smooth (section 3.4) Probabilistic Continuous

3rd /4th order continuous 2nd /3rd order continuous Start posture Target posture

Start pose Target position

No

Yes

3rd order continuous 2nd order continuous Start posture Target posture with θ range Yes

Low

High

Low

Curvature constraint Comparative Efficiency

4.3

RG-RRT

Step function Discontinuity

Continuous Start pose Target position Yes High

Comparison of Developed Path Planning Methods

As discussed in Chapter 3, four different algorithms are developed, for the continuous curvature path planning of neurosurgical flexible probe under curvature constraint. It is interesting to make a comparison of these developed approaches, as each algorithm has its own advantages and disadvantages. Summary of comparison is shown in Table 4.1, which highlights some important features of the developed algorithms. These features are discussed below in detail: Nature of Algorithm As listed in Table 4.1, all algorithms are of probabilistic nature except for the gradient method. This is because they make use of the Rapidly-exploring Random Tree(RRT), hence every time the algorithm is executed, a unique path is obtained. Nature of Curvature and Derivative of Curvature The curvature of the RG-RRT based approach has discrete steps, which makes its derivative to be discontinuous. A path which is not smooth is not appropriate for the neurosurgical flexible probe, as its mechanical constraints does not allow to make drastic changes in the


56

curvature. Comparatively, all other developed approaches gives paths with continuous curvature and derivative of curvature, thus giving a smooth path. This is listed in Table 4.1. Set of Input Parameters The RG-RRT and Globally smooth path planner requires only start pose (x, y, θ) and target position (x, y) as its input, while the Gradient method and Locally smooth planner requires the start and target postures (x, y, θ, κ) as its inputs. The value of input and output curvature κ can be fixed to a certain value by the user, but a range of target orientation θ needs to be defined, which makes these algorithms sometimes difficult to handle by the user. It might leads to no solution, if the range of target orientation θ is not defined properly. Curvature Constraint Developing a path planning algorithm with constraint curvature was one of the task to be achieved in this research work. It is observed that the curvature can be constraint in all developed approaches except for the gradient method. This is because geometric approach is deterministic and always leads to one unique solution. However, by varying the target orientation, the curvature can be changed. This can be observed from Figure 4.2. But such changes involves user interference with the algorithm for tuning it manually to get the required result. All other three approaches, namely RG-RRT, Locally Smooth and Globally Smooth path planning constraints the curvature within the define bound, thus giving them an extra edge over Gradient method. Comparative Efficiency Though an exact timing analysis can not be perform as the algorithms are of probabilistic and iterative nature, but it is observed that Gradient method and locally smooth path planner takes more time than RG-RRT and Globally smooth path planner. This is because in Gradient method, a great number of iterations and mathematical integrals are involved in computation. Moreover, if the values of µ and h are not tuned properly, it becomes even more slower. In case of Locally Smooth path planner, since it is developed by the integration of RG-RRT with Gradient method, hence it is even more slower, as it calculates gradient based solution between two nodes, multiple times.

4.4

Discussion

Testing of developed methods on provided brain riskmap was successfully done and detailed analysis of path optimization approach was presented. It is observed that Globally smooth

57

4.4 Discussion

path planner outperformed all other developed approaches, namely, the gradient based and RGRRT based and locally smooth path planner. It generated paths, which are smooth, i.e. their curvature and derivative of curvature are continuous. The generated path is well constraint within the curvature limits. Moreover, it is efficient in comparison to other methods, hence making it possible to develop a real time software for path generation of neurosurgical flexible probe in future.

Chapter 5

Conclusions This chapter details the conclusions and future work for path planning of the neurosurgical flexible probe.

5.1

Conclusions

In this thesis, continuous curvature path planning methods have been developed, for steering a neurosurgical flexible probe. The flexible probe is capable of steering in two dimensional space, can be modelled as a nonholonomic robot. A constraint on the minimum radius of curvature was imposed on the probe, due to its mechanical limits, thus only allowing it to follow curvilinear paths. Further, the kinematics of the probe require that the curvature is continuous and smooth. Path planning methods have been developed based on the mechanical constraints of the flexible probe, for computing the optimal path with minimum risk to the patient. Different approaches have been investigated for solving this problem. An existing Gradient based method for solving the continuous curvature path planning problem for a nonholonmic robot was implemented, but failed to bound the path’s curvature. A Rapidly-exploring Random Tree (RRT) combined with a reachability guided heuristic was then applied to the given problem, successfully bounding the curvature, but the generated path were composed of circular arcs, hence the continuity condition for the curvature was not satisfied. With the integration of the Gradient based method with the Reachability Guided Rapidly-exploring Random Tree (RG-RRT), a Locally Smooth path planning approach was developed. This approach gave bounded continuous curvature paths, but manual tuning of certain parameters was required, otherwise, the approach becomes inefficient and might not return a solution. Finally, a Globally Smooth path planner was designed by combining RG-RRT with a smoothing approach. The smoothing ap58

59

5.2 Future Work

proach was developed by adapting bézier curve mathematics, which is a curve approximation technique widely used in computer graphics. The designed Globally Smooth path planner gives an innovative solution towards path planning for the flexible probe and is able to compute bounded curvature continuous and smooth paths. A path optimization scheme was also developed, which computed the best path based on predefined criteria, such as, minimum damage to the tissue, maximum clearance from obstacles and minimum risk to the patient, for selecting optimal path. Results showed that the globally smooth path planner was more efficient and effective than the other three developed path planners,i.e. , the gradient based, sampling based and locally smooth path planner and it satisfied the nonholonomic constraint of the neurosurgical flexible probe.

5.2

Future Work

This thesis successfully accomplished the assigned task, however there are still several aspects which can be further investigated in the context of path planning of the neurosurgical flexible probe. In the current study, the environment was considered static but in reality, inserting and retracting the flexible probe causes the surrounding soft tissue to displace and deform, neglecting these deformations can result in substantial placement error. Thus, an approach is required in order to compensate for tissue deformation during insertion. Implementation of the Globally Smooth path planner in C++ environment can be helpful in increasing its efficiency. Development of an online control for the flexible probe, for the correction of placement error during steering is possible by using feedback from the real-time 2D CT/MRI brain images. This can facilitate in rapidly replanning of a path when unexpected events occur due to tissue inhomogeneity and tissue deformation. The neurosurgical flexible probe design allow it to steer in 3D space as well, thus extending the developed continuous curvature path planner for 3D environment is highly recommended.

Bibliography [1] M. A. Talamini, Advanced Therapy With Minimally Invasive Surgery. PMPH-USA, 2006. [2] K. Fuchs, “Minimally invasive surgery,” Endoscopy, vol. 34, no. 2, pp. 154–159, 2002. [3] B. Badie, N. Brooks, and M. Souweidane, “Endoscopic and minimally invasive microsurgical approaches for treating brain tumor patients,” Journal of neuro-oncology, vol. 69, no. 1, pp. 209–219, 2004. [4] L. Frasson, S. Ko, A. Turner, T. Parittotokkaporn, J. Vincent, and F. Rodriguez y Baena, “STING: a soft-tissue intervention and neurosurgical guide to access deep brain lesions through curved trajectories,” Proceedings of the Institution of Mechanical Engineers, Part H: Journal of Engineering in Medicine, vol. 224, no. 6, pp. 775–788, 2010. [5] R. Webster, J. Kim, N. Cowan, G. Chirikjian, and A. Okamura, “Nonholonomic modeling of needle steering,” The International Journal of Robotics Research, vol. 25, no. 5-6, p. 509, 2006. [6] S. Ko, B. Davies, F. Rodriguez y Baena, et al., “Two-dimensional needle steering with a programmable bevel inspired by nature: Modeling preliminaries,” in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on, pp. 2319–2324, IEEE. [7] J. Engh, G. Podnar, D. Kondziolka, and C. Riviere, “Toward effective needle steering in brain tissue,” in 28th Annual International Conference of the IEEE Engineering in Medicine and Biology Society, 2006. EMBS’06., pp. 559–562, IEEE, 2006. [8] S. Patil and R. Alterovitz, “Interactive motion planning for steerable needles in 3D environments with obstacles,” in 3rd IEEE RAS and EMBS International Conference on Biomedical Robotics and Biomechatronics (BioRob), 2010, pp. 893–899, IEEE, 2010. [9] J. Xu, V. Duindam, R. Alterovitz, J. Pouliot, J. Cunha, I. Hsu, and K. Goldberg, “Planning fireworks trajectories for steerable medical needles to reduce patient trauma,” in IEEE/RSJ 60

61

BIBLIOGRAPHY

International Conference on Intelligent Robots and Systems, 2009. IROS 2009., pp. 4517– 4522, IEEE, 2009. [10] R. Shamir, L. Joskowicz, S. Spektor, and Y. Shoshan, “Localization and registration accuracy in image guided neurosurgery: a clinical study,” International journal of computer assisted radiology and surgery, vol. 4, no. 1, pp. 45–52, 2009. [11] L. Frasson, T. Parittotokkaporn, A. Schneider, B. Davies, J. Vincent, S. Huq, P. Degenaar, and F. Rodriguez y Baena, “Biologically inspired microtexturing: Investigation into the surface topography of next-generation neurosurgical probes,” in 30th Annual International Conference of the IEEE Engineering in Medicine and Biology Society, 2008. EMBS 2008., pp. 5611–5614, IEEE, 2008. [12] T. Parittotokkaporn, L. Frasson, A. Schneider, S. Huq, B. Davies, P. Degenaar, J. Biesenack, and F. Rodriguez y Baena, “Soft tissue traversal with zero net force: Feasibility study of a biologically inspired design based on reciprocal motion,” in IEEE International Conference on Robotics and Biomimetics, 2008. ROBIO 2008., pp. 80–85, IEEE, 2008. [13] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations. Cambridge, MA: MIT Press, June 2005. [14] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge University Press, 2006. Available at http://planning.cs.uiuc.edu/. [15] N. Sariff and N. Buniyamin, “An overview of autonomous mobile robot path planning algorithms,” in 4th Student Conference on Research and Development, 2006. SCOReD 2006., pp. 183–188, IEEE, 2006. [16] J. Latombe, Robot motion planning. Springer Verlag, 1990. [17] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” The international journal of robotics research, vol. 5, no. 1, p. 90, 1986. [18] I. Sucan and L. Kavraki, “On the implementation of single-query sampling-based motion planners,” in IEEE International Conference on Robotics and Automation (ICRA), 2010, pp. 2005–2011, IEEE, 2010. [19] L. Kavraki, P. Svestka, J. Latombe, and M. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.

BIBLIOGRAPHY

62

[20] R. Geraerts and M. Overmars, “A comparative study of probabilistic roadmap planners,” Algorithmic Foundations of Robotics V, pp. 43–58, 2003. [21] C. Nielsen and L. Kavraki, “A two level fuzzy prm for manipulation planning,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2000.(IROS 2000). Proceedings. 2000, vol. 3, pp. 1716–1721, IEEE, 2000. [22] R. Bohlin and L. Kavraki, “Path planning using lazy prm,” in IEEE International Conference on Robotics and Automation, 2000. Proceedings. ICRA’00., vol. 1, pp. 521–528, IEEE, 2000. [23] S. LaValle and J. Kuffner, “Rapidly-exploring random trees: Progress and prospects,” in Algorithmic and computational robotics: new directions: the fourth Workshop on the Algorithmic Foundations of Robotics, p. 293, AK Peters, Ltd., 2001. [24] S. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” In, no. 98-11, 1998. [25] S. Rodriguez, X. Tang, J. Lien, and N. Amato, “An obstacle-based rapidly-exploring random tree,” in Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006., pp. 895–900, IEEE, 2006. [26] M. Kalisiak and M. van de Panne, “Rrt-blossom: Rrt with a local flood-fill behavior,” in Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006., pp. 1237–1242, IEEE, 2006. [27] A. Shkolnik, M. Walter, and R. Tedrake, “Reachability-guided sampling for planning under differential constraints,” in IEEE International Conference on Robotics and Automation, 2009. ICRA’09., pp. 2859–2865, IEEE, 2009. [28] S. Carpin, “Randomized motion planning: A tutorial,” International Journal of Robotics and Automation 2006, vol. 21, no. 3, 2006. [29] L. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, no. 3, pp. 497–516, 1957. [30] J. Laumond, P. Jacobs, M. Taix, and R. Murray, “A motion planner for nonholonomic mobile robots,” IEEE Transactions on Robotics and Automation, vol. 10, no. 5, pp. 577– 593, 1994.

63

BIBLIOGRAPHY

[31] A. Scheuer and T. Fraichard, “Continuous-curvature path planning for car-like vehicles,” in Proceedings of the 1997 IEEE/RSJ International Conference on Intelligent Robots and Systems, 1997. IROS’97, vol. 2, pp. 997–1003, IEEE, 1997. [32] Y. Suzuki, S. Kagami, and J. Kuffner, “Path planning with steering sets for car-like robots and finding an effective set,” in IEEE International Conference on Robotics and Biomimetics, 2006. ROBIO’06., pp. 1221–1226, IEEE. [33] R. Alterovitz, M. Branicky, and K. Goldberg, “Constant-curvature motion planning under uncertainty with applications in image-guided medical needle steering,” Algorithmic Foundation of Robotics VII, pp. 319–334, 2008. [34] B. Nagy and A. Kelly, “Trajectory generation for car-like robots using cubic curvature polynomials,” Field and Service Robots, vol. 11, 2001. [35] S. Thompson and S. Kagami, “Continuous curvature trajectory generation with obstacle avoidance for car-like robots,” International Conference on Computational Intelligence for Modelling, Control and Automation and International Conference on Intelligent Agents, 2005. [36] O. Lefebvre, F. Lamiraux, C. Pradalier, and T. Fraichard, “Obstacles Avoidance for CarLike Robots,” in International Conference on Robotics and Automation (ICRA), Citeseer, 2004. [37] J. Xu, V. Duindam, R. Alterovitz, and K. Goldberg, “Motion planning for steerable needles in 3D environments with obstacles using rapidly-exploring random trees and backchaining,” in IEEE International Conference on Automation Science and Engineering, 2008. CASE 2008, pp. 41–46, IEEE, 2008. [38] C. Caborni, Curvilinear Path Planning for a Steerable Flexible Neuro-surgical Probe. Politecnico di Milano, A.A, MSc Thesis 2010-11. [39] K. Yang and S. Sukkarieh, “An analytical continuous-curvature path-smoothing algorithm,” IEEE Transactions on Robotics, vol. 26, no. 3, pp. 561–568, 2010. [40] K. Yang and S. Sukkarieh, “3d smooth path planning for a uav in cluttered natural environments,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008. IROS 2008., pp. 794–800, IEEE, 2008. [41] Y. Qu, Q. Pan, and J. Yan, “Flight path planning of uav based on heuristically search and genetic algorithms,” in Industrial Electronics Society, 2005. IECON 2005. 31st Annual Conference of IEEE, pp. 5–pp, IEEE, 2005.

BIBLIOGRAPHY

64

[42] R. Beard, T. McLain, M. Goodrich, and E. Anderson, “Coordinated target assignment and intercept for unmanned air vehicles,” IEEE Transactions on Robotics and Automation, vol. 18, no. 6, pp. 911–922, 2002. [43] P. Pettersson and P. Doherty, “Probabilistic roadmap based path planning for an autonomous unmanned helicopter,” Journal of Intelligent and Fuzzy Systems, vol. 17, no. 4, pp. 395–405, 2006. [44] K. Yang and S. Sukkarieh, “Planning continuous curvature paths for uavs amongst obstacles,” in Australas. Conf. Robot. autom., Canberra, Australia, 2008. [45] P. Shirley, Fundamentals of computer graphics. Wellesley, Mass: Peters, 2005. [46] B. Rieger, Machine Learning and Systems Engineering. Springer, 2010. [47] Y. Kanayama and B. Hartman, “Smooth local path planning for autonomous vehicles,” in IEEE International Conference on Robotics and Automation, 1989. Proceedings., 1989, pp. 1265–1270, IEEE, 1988. [48] Reuben R. Shamir, Leo Joskowicz, Luca Antiga, Roberto I. Foroni, Yigal Shoshan, “Trajectory planning method for reduced patient risk in image-guided neurosurgery: concept and preliminary results,” in SPIE Medical Imaging, 2010. [49] E. D. L. J. Y. S. Reuben R. Shamir, Idit Tamir, “A method for planning safe trajectories in image-guided keyhole neurosurgery,” in the 13th International Conference on Medical Image Computing and Computer Assisted Intervention, MICCAI 2010, 2010.

Continuous Curvature Path Planning for a

Continuous Curvature Path Planning for a

Suggest Documents

Piecewise Bezier Curves Path Planning with Continuous Curvature ...

A memetic algorithm for path planning of curvature-constrained ... - Core

Planning Continuous Curvature Paths Using Constructive Polylines

Continuous Path Planning for a Data Harvesting Mobile Server

Planning Continuous Curvature Paths for UAVs Amongst Obstacles

Planning Continuous-Curvature Paths for Car-Like ... - Semantic Scholar

Combining Task and Path Planning for a

a potential-based path planning algorithm for

a potential-based path planning algorithm for

Single-Agent On-line Path Planning in Continuous, Unpredictable and ...

Perception-aware Path Planning

Roadmap-Based Path Planning

A Cooperative Path Planning Algorithm for a Multiple ... - SAGE Journals

NONHOLONOMIC, BOUNDED CURVATURE PATH ... - Centro Piaggio

Continuous-Discrete Path Integral Filtering

A Model Algorithmic Learning Method for Continuous Path ... - Incorl

Continuous Curvature Trajectory Design and ... - IEEE Xplore

Asymptotically-optimal Path Planning for Manipulation using ...

Collision-Free Path and Motion Planning for

Multiresolution Hierarchical Path-Planning for Small UAVs

Three-dimensional Path Planning for Unmanned ...

Three-Dimensional Path Planning Method for Autonomous ...

Onboard Path Planning for Reusable Launch ...

Path Planning for Network Performance - Lex Fridman