US20100121148A1

US20100121148A1 - Method and system for steerable medical device path definition and following during insertion and retraction

Info

Publication number: US20100121148A1
Application number: US12/613,739
Authority: US
Inventors: Caitlin Quance Donhowe; Amir Belson; Kristoffer John Donowe; Kenneth Robert Krieg; Eric Michael Storne; Thomas J. Yorkey; Jun Zhang
Original assignee: Intuitive Surgical Inc
Current assignee: Intuitive Surgical Operations Inc
Priority date: 2008-11-11
Filing date: 2009-11-06
Publication date: 2010-05-13
Also published as: US20210038055A1; US20180333039A1

Abstract

Waypoints for a steerable medical device are stored as the steerable medical device is moved within a patient. The stored waypoints are an ordered sequence of locations. The ordered sequence of locations defines a safe path within the patient for moving an articulatable portion of the steerable medical device. The articulatable portion of the steerable medical device is constrained to follow the safe path as the articulatable portion moves within the patient. For example, the articulatable portion of the steerable medical device is constrained to remain within a boundary region enclosing the safe path as the articulatable portion of the steerable medical device follows the safe path.

Description

RELATED APPLICATION

The present application claims the benefit of U.S. Provisional Patent Application No. 61/113,534 filed Nov. 11, 2008 entitled “Method for Robotic Endoscope Path Definition and Following During Insertion and Retraction,” naming as inventors, Caitlin Donhowe, Jun Zhang, Kristoffer Donhowe, Eric Storne, Thomas Yorkey, Kenneth R. Krieg, and Amir Belson, the entirety of which is hereby incorporated by reference.

BACKGROUND

1. Field of Invention
Aspects of this invention are related to medical device path planning, and more particularly are related to path planning for an articulatable multi-segment medical device.
2. Related Art
An endoscope is a medical device for visualizing the interior of a patient's body. Endoscopes have been used for a variety of medical diagnostic procedures and for a variety of medical interventional procedures.
Many different types of endoscopes are known. For example, one steerable endoscope has an elongated body with a steerable distal portion and an automatically controlled proximal portion. Such an endoscope is described in U.S. Pat. No. 6,468,203 B2, entitled “Steerable Endoscope and Improved Method of Insertion,” of Amir Belson issued on Oct.22, 2002, which is incorporated herein by reference in its entirety.
In general, conventional medical device path-planning is performed using medical imaging data usually CAT, X-ray, MRI, PET of fluoroscopy imaging data. Path-planning is considered in U.S. Pat. No. 5,611,025, entitled “Virtual internal cavity inspection system,” issued Mar. 11, 1997; U.S. Pat. No. 7,167,180, entitled “Automatic path planning system and method,” issued Jan. 23,2007; U.S. Pat. No. 6,380,958, entitled “Medical-technical System,” issued Apr. 30, 2002; U.S. Patent Application Publication No. US 2008/0091340 A1 (filed Dec. 26, 2004, disclosing “Targeted Marching”); and U.S. Patent Application Publication No. US 2003/0109780 A1 (filed Jun. 6, 2002, disclosing “Methods and Apparatus for Surgical Planning”).

SUMMARY

Unlike prior art methods that required path planning prior to use of a steerable medical device, waypoints of a steerable medical device are stored as the device is moved within a patient. The stored waypoints are an ordered sequence of locations. The ordered sequence of locations defines a safe path within the patient for moving an articulatable portion of the steerable medical device.
Thus, the articulatable portion of the steerable medical device is constrained to follow the safe path as the articulatable portion moves within the patient or as the device is inserted into the patient. In one aspect, the articulatable portion of the steerable medical device is constrained to remain within a boundary region enclosing the safe path.
In one embodiment, the articulatable portion of the steerable medical device includes a segment of the steerable medical device. The segment, in turn, includes a plurality of links.
In another aspect, the boundary region is a tube having a cross-section. Examples of cross-sections include, but are not limited to, a circular cross-section, an oblate cross-section, and a rectangular cross cross-section.
In still yet another aspect, constraining the articulatable portion of the steerable medical device to remain within a boundary region enclosing the safe path includes generating position and orientation data for each of the plurality of segments. This process uses the ordered sequence of locations and a kinematic model of the plurality of segments of the steerable medical device.
The process of generating position and orientation data for each of the plurality of segments includes minimizing a cost function. In one aspect, minimizing a cost function further includes minimizing the sum of the absolute values of relative joint angles, with an additional constraint that link positions must remain within some distance Δ of the safe path formed by the waypoints.
An apparatus includes a steerable medical device having an articulatable portion. The articulatable portion includes at least one articulatable segment. The apparatus also includes a controller, coupled to the steerable medical device. The controller includes a processor, and a medical device controller coupled to the processor and to the steerable medical device. The processor stores in the memory an ordered sequence of locations of waypoints. The processor also analyzes the ordered sequence of locations and outputs to the medical device controller at lease one command to constrain the articulatable portion within a boundary region enclosing a safe path defined by the ordered sequence of location.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a diagrammatic view of a steerable medical device including an articulatable portion, and a safe path.

FIG. 2 is a diagrammatic view of an apparatus used in one aspect of the processes and operations described herein.

FIGS. 3A to 3C are diagrammatic views of boundary regions enclosing a safe path.

FIG. 4 is a process flow diagram for one embodiment of a method of configuring a steerable medical device to follow a safe path.

FIG. 5 is a diagrammatic view of elements of a kinematic model that can be used in the method of FIG. 4.

In the drawings, the first digit of a figure number indicates the figure in which the element with that figure number first appeared.

DETAILED DESCRIPTION

In one aspect, a steerable medical device 100 (FIG. 1), sometimes referred to as medical device 100, includes a plurality of segments 101-1, 101-2, 101-3 . . . In the embodiment of FIG. 1, at least three segments 101-1, 101-2, 101-3 include a plurality of movable links 102-1, 102-2 and 102-3, respectively. In one aspect, a tip of steerable medical device 100 is at distal end 120 and typically includes at least a camera.
The use of a steerable medical device with at least three segments each having a plurality of movable links is illustrative only and in not intended to be limiting. In view of this disclosure, a steerable medical device may include from at least one segment having a plurality of links to a number of segments with movable links needed to provide the required functionality for the steerable medical device.
An example of a steerable medical device 100 is a steerable endoscope. In the following description, steerable medical device 100 is referred to as steerable endoscope 100. The use of steerable endoscope 100 as an example of a steerable medical device is illustrative only and is not intended to be limiting to this specific steerable medical device.
The plurality of movable links in a segment allows that segment to be articulated as steerable endoscope 100 moves into and out of a patient. Thus, such segments are referred to as articulatable segments.
In one aspect, an operator manipulates a direction controller 245 (FIG. 2) to move the tip of steerable endoscope 100 in a particular direction within the patient. In response to commands from a processor in processor module 250, as described more completely below, and a signal from direction controller 245, an endoscope controller 240 (FIG. 2) for steerable endoscope 100 configures each of the links so that each segment is constrained to move along, e.g., to follow, a known safe path 150 (FIG. 1) that is defined, in one aspect, by movement of the tip of endoscope 100.
In one embodiment, as the operator steers the tip of steerable endoscope 100 through the patient, a safe path is generated. More specifically, as the operator inserts steerable endoscope 100 into a patient and moves steerable endoscope 100 towards a desired location, an ordered sequence of locations {P_k, P_k+1, P_k+2, P_k+3, P_k+4} (FIG. 1) of the tip of endoscope 100 are recorded in a memory 230 (FIG. 2) as waypoint data 231, in this example.
The ordered sequence of locations {P_k, P_k+1, P_k+2, P_k+3, P_k+4} (FIG. 1) represent the motion of the tip of steerable endoscope 100 through the patient. Thus, the ordered sequence of locations {P_k, P_k+1, P_k+2, P_k+3, P_k+4} represent a safe path 150, which has been chosen by the operator, for steerable endoscope 100 to follow as steerable endoscope 100 is inserted into and withdrawn from the patient.
Using the tip movement to generate an ordered sequence of locations that define safe path 150 is illustrative only and is not intended to be limiting. Alternatively, steerable endoscope 100 could be inserted in a passive state, and then all the inserted articulatable segments activated at one. The positions of the links in the articulatable segments at the time of activation provide an ordered sequence of locations that define safe path 150 for additional insertion and for retraction of steerable endoscope 100.
In one aspect, as described more completely below, all trailing segments 101-1, 101-2, 101-03 . . . of steerable endoscope 100 are constrained to follow safe path 150 that is defined by ordered sequence of locations {P_k, P_k+1, P_k+2, P_k+3, P_k+4}. However, trailing segments 101-1, 101-2, 101-03 . . . may not follow safe path 150 exactly. In one aspect, the operator can define a boundary region around safe path 150. The boundary region (See FIGS. 3A to 3C) is a volume inside which steerable endoscope 100 can move safely. In one aspect, all trailing segments 101-1, 101-2, 101-03 . . . are constrained to remain within this boundary region as steerable endoscope 100 moves into the patient.
Also, in another aspect, as medical device 100 is withdrawn from the patient, articulatable segments 101-1, 101-2, 101-03 . . . are constrained to remain within this boundary region. Hence, unlike prior art techniques that required advance planning, a safe route for medical device 100 into and out of a patient is generated based on waypoint data 231 recorded as steerable endoscope 100 moves into the patient.
When segment 101-1 is considered a trailing segment, as just described, segment 101-1 is controlled in the same way as the other trailing articulatable segments. However, in another insertion mode, segment 101-1 remains directly controlled by the operator and does not change shape with insertion. In this insertion mode, segment 101-1 would not be considered a trailing segment.
FIGS. 3A to 3C are illustrations of boundary regions 310A to 310C, with different cross-sectional shapes, for safe path 150 defined by ordered sequence of locations {P_k, P_k+1, P_k+2, P_k+3, P_k+4}. In FIG. 3A, boundary region 310A has a circular cross-section. In FIG. 3B, boundary region 310B has an oblate cross-section. In FIG. 3C, boundary region 310C has a rectangular cross-section, and in particular, a specific instance of a rectangular cross-section, a square cross-section.
In one aspect, each location P_kis a point in space (x_k, y_k, z_k) relative to a fixed reference frame. Thus, each of boundary regions 310A, 310B, 310C defines a three-dimensional volume enclosing safe path 150. This volume is referred to as a tube having a cross section to convey the three-dimensional characteristics of the volume. Tube, as used here, is not a physical tube inserted in the patient to define the volume.
The particular cross-sectional shapes of the boundary regions illustrated in FIGS. 3A to 3C are illustrative only and are not intended to be limiting. In view of this disclosure, for a particular medical procedure and steerable medical device, an appropriately shaped boundary region or regions can be selected.
It is not necessary that the boundary region have the same cross-sectional shape over the entire extent of the safe path. For example, a first portion of a boundary region could have a first cross-sectional shape, and a second portion of the boundary region could have a second cross-sectional shape that is different from the first cross-sectional shape.
FIG. 4 is a process flow diagram for one embodiment of a method 400 used with a steerable medical device such as steerable endoscope 100. In one aspect, instructions in a METHOD 400 module 232 (FIG. 2) in memory 230 are executed on a processor in processor module 250 to perform at least some of the operations described more completely below.
In method 400, STORE WAYPOINTS operation 420, as described more completely below, saves waypoints of the tip of steerable endoscope 100 as an ordered sequence of locations. The ordered sequence of locations represents the trajectory of the tip as steerable endoscope 100 is inserted into the patient. As described above, the ordered sequence of locations defines a safe path 150 that has been selected by the operator of steerable endoscope 100.
The stored ordered sequence of locations is retrieved and analyzed in FOLLOW SAFE PATH process 430. FOLLOW SAFE PATH process 430 configures the articulatable portion of endoscope 100 so that the articulatable portion of endoscope 100 is constrained to follow safe path 150 as the articulatable portion moves within the patient. Herein, following the safe path does not mean that the safe path is followed exactly, but rather that the articulatable portion steerable endoscope 100 stays at least within a safe region, sometimes called a boundary region, about safe path 150.
In FIG. 4, one embodiment of STORE WAYPOINTS operation 420 and FOLLOW SAFE PATH process 430 are illustrated and described more completely below. This embodiment is illustrative only and is not intended to be limiting to the specific operations and processes described. In view of this disclosure, one knowledgeable in the field can utilize other techniques to move the articulatable portion of a steerable medical into and out of a patient while following the safe path described herein.
In FIG. 4, upon initiation of method 400, an INTIALIZE operation 401 is performed. INTIALIZE operation 401 initializes any variables, memory structures etc that are need for subsequent operations in method 400. In one aspect, INITIALIZE operation 401 uses display controller 235 (FIG. 2) to generate a display on display device 210 that permits the operator of steerable endoscope 100 to select parameters that define the extent of the boundary region, for example. Upon completion INTIALIZE operation 401 (FIG. 4) transfers processing to ESTABLISH WAYPOINT operation 402 in STORE WAYPOINTS operation 420.
In ESTABLISH WAYPOINT operation 402, a waypoint on the safe path of the endoscope is established. This can be done in a variety of ways. Accordingly, check operations 410 to 412, as discussed more completely below, are illustrative only and are not intended to be limiting to this particular implementation.
In one aspect, as the operator inserts steerable endoscope 100 into a body cavity (for instance, through an incision in the stomach into the abdominal cavity) insertion is paused periodically. When motion of endoscope 100 is paused, DEVICE MOTION PAUSED check operation 410 transfers processing to DEVICE MOTION RESUMED check operation 411.
If steerable endoscope 100 is paused to determine a safe trajectory for the tip, the operator uses the camera and illumination at the tip of steerable endoscope 100 to determine a safe trajectory inside the body cavity. When viewing the interior of the body cavity and determining a safe direction for farther insertion, the endoscope tip may be oriented in any direction to provide views of the body cavity.
Once a safe trajectory is determined, the tip is oriented in the direction that the operator intends steerable endoscope 100 to travel upon farther insertion. The operator then inserts endoscope 100 farther into the patient. However, the operator could also retract endoscope 100 after the pause. Thus, upon resumption of motion of endoscope 100, DEVICE MOTION RESUMED check operation 411 transfers processing to INSERTION check operation 412.
INSERTION check operation 412 determines whether after the pause, the operation is continuing to insert endoscope 100 or is retracting endoscope 100. If the operator is continuing to insert endoscope 100, check operation 412 transfers processing to SAVE WAYPOINT operation 430. Conversely, if the operator is retracting endoscope 100, check operation 412 transfers to DEVICE MOTION PAUSED check operation 410 and to MAINTAIN SEGMENTS IN BOUNDARY REGION operation 405.
Again, the combination of operations 410 to 412 is one way to allow an operator to establish a waypoint. Alternatively, the operator could, for example, push a button when a safe trajectory has been identified and the operator is about to move the endoscope tip on that trajectory.
SAVE WAYPOINT operation 403 records the previous location of the tip as a waypoint in WAYPOINT DATA 403a, which in FIG. 2 is shown as waypoint data 231. SAVE WAYPOINT operation 403, upon completion, transfers processing to NEW LINK INSERTED check operation 404. SAVE WAYPOINT operation 403 also transfers back to the start of ESTABLISH WAYPOINT operation 402 in case the operator decides to change the direction of the safe trajectory.
In this example, insertion of a link is used as a trigger to record another waypoint for the tip. However, other criteria can be used so long as locations of the tip are saved with sufficient resolution to permit determining a safe path. As steerable endoscope 100 is moved further into the patient along the safe trajectory, as each new link is inserted in the patient, the location of the tip is saved as another waypoint. In this example, when a new link is inserted, NEW LINK INSERTED check operation 404 transfers processing to SAVE WAYPOINT operation 403, which was described above, and to UPDATE check operation 409.
UPDATE check operation 409 is shown with a dotted line in FIG. 4 because check operation 409 is optional. In one aspect, the configuration of the trailing segments of endoscope 100 is updated only after a predefined number of new waypoints have been saved. In this aspect, UPDATE check operation 409 does not transfer processing to process 405 until the predefined number of new waypoints is available in WAYPOINT DATA 403A. Also, UPDATE check operation 409 checks the states of the articulatable segments and if none of the articulatable segments have state articulating, UPDATE check operation 409 does not transfer processing to process 405. Alternatively, the configuration of the trailing segments of endoscope 100 is updated for each new waypoint and so UPDATE check operation 409 is not needed.
In this embodiment, FOLLOW SAFE PATH process 430 includes MAINTAIN SEGMENTS IN BOUNDARY REGION process 405 and SEND COMMAND process 406. MAINTAIN SEGMENTS IN BOUNDARY REGION process 405 uses the saved waypoints to generate a configuration for each of the inserted links so that trailing segments 101-1 to 101-3 follow safe path 150 of the tip as defined by the waypoints. As noted above, for some steerable medical devices, it may not be possible to configure the trailing segments to follow exactly safe path 150. Thus, in one aspect, a boundary region about the ordered sequence of locations {P_k}_k=1 ^Nis generated. Here, P_kdenotes the Cartesian coordinates of the endoscope tip when the number of the inserted links is k. After the endoscope has been inserted for N links, the path trajectory of the endoscope tip is recorded as ordered sequence of locations {P_k}_k=1 ^Nin memory 230 (FIG. 2).
As described above, the boundary region defines a volume around safe path 150, i.e., a volume that encloses safe path 150. The trailing segments can safely pass through any part of the boundary region as the trailing segments move towards the surgical site, or move away from the surgical site.
In one aspect, a kinematic model of steerable endoscope 100, such as model 500 in FIG. 5, is used in MAINTAIN SEGMENTS IN BOUNDARY REGION process 405. In kinematic model 500, (FIG. 5), a first level 501 of model 500 defines the number of articulatable segments in a plurality of articulatable segments of the steerable endoscope. In this example, the steerable endoscope has four articulatable segments Seg 1, Seg 2, Seg 3, and Seg 4.
A second level 502 of model 500 is a representation of links and joints in each of four articulatable segments Seg 1, Seg 2, Seg 3, and Seg 4. In this example, each segment includes eight links, where adjacent links are separated by a joint. The joints are numbered sequentially from the distal end to the proximal end of the endoscope, starting with a value one. (This numbering sequence is for convenience only. In other aspects, the numbering may increase in going from a proximal location towards a distal location.) Each link has a fixed length and the associated joint can rotate in a single plane, either the x-plane or the y-plane. The joints alternate so adjacent links rotate in different planes.
In model 500, each of four articulatable segments Seg 1, Seg 2, Seg 3, and Seg 4 has an angle of rotation about the x-axis and an angle of rotation about the y-axis. For articulatable segment Seg 1, the angle of rotation about the x-axis is θ_y ¹, (sometimes referred to as pitch) and the angle of rotation about the y-axis is θ_x ¹(sometimes referred to as yaw). For articulatable segment Seg 2, the angle of rotation about the x-axis is θ_y ², and the angle of rotation about the y-axis is θ_x ². For articulatable segment Seg 3, the angle of rotation about the x-axis is θ_y ³, and the angle of rotation about the y-axis is θ_x ³. For articulatable segment Seg 4, the angle of rotation about the x-axis is θ_y ⁴, and the angle of rotation about the y-axis is θ_x ⁴.
Row 505 in model 500 gives the initial constraint on the joint angles. In this example, the joint angles for a segment are assumed to be equal. Thus, each joint angle is one-fourth of the corresponding segment angle. Row 506 in model 500 gives the roll matrix for each joint.
In this model, y-links are links that cause motion in the y-direction rather than links that rotate about the y-direction. This definition results in R_x(θ_y). Similarly, in this model, x-links are links that cause motion in the x-direction rather than links that rotate about the x-direction. This definition results in R_y(θ_x) Those knowledgeable in the field understand that alternative definitions can be used in the kinematic model, where x-links are links that rotate about the x-direction and y-links are links that rotate about the y-direction. Irrespective of the kinematic model definitions, the results are equivalent.
Thus, model 500 is illustrative only and is not intended to be limiting. Model 500 is used in one example that is developed more completely below.
Using stored ordered sequence of locations {P_k}_k=1 ^Nand kinematic model 500, a cost function is minimized to generate position and orientation data of the endoscope segments in MAINTAIN SEGMENTS IN BOUNDARY REGION process 405. Specifically, in one aspect, positions and orientations are generated for each of the links in the articulatable portion of endoscope 100. The positions and orientations are constrained within the boundary region.
The cost function allows the position and orientation to be optimized for a variety of criteria, for example, but not limited to: 1) maximum tip controllability; 2) maximum distance of the segments from inverse-kinematic singularities; 3) maximum distance of the segments from articulation limits; 4) maximum distance from user-defined body-cavity boundaries (organs, adhesions, etc.); 5) minimum distance from the defined path.
In process 405, the cost function may be a dynamically changing function of any number of criteria. However, the number of criteria is selected so the cost function can be minimized and the segments positioned to achieve the constrained movement within an endoscope movement time acceptable to the operator. Conventional techniques are used by a processor to minimize the cost function selected subject to the position constraints imposed.
In an additional aspect, an embodiment of process 405 determines the optimal configuration of the segments (relative segment angles) to satisfy the constraint that the segment links stay “near to” the path defined by the tip forward motion and any additional constraints on the link (or segment) positions.
For example, in process 405, the segments may be allowed to assume positions within a tubular bounding region 310A (FIG. 3A) around the tip path while encouraging tip manipulability. One way to encourage tip manipulability in process 405 is to minimize the sum of the absolute values of the relative joint angles, with the additional constraint that the link positions not deviate by more than a distance at each of the waypoints in ordered sequence of locations {P_k}_k=1 ^N. In one aspect, distance Δ is specified by the operator.
Upon determining the configuration for each of the articulatable segments, MAINTAIN SEGMENTS IN BOUNDARY REGION process 405 transfers processing to SEND COMMAND process 406 (FIG. 4). In SEND COMMAND process 406, the processor in processor module 250 (FIG. 2) sends at least one command to the motors in endoscope controller 240 to configure the articulatable segments based on the locations and orientations generated in process 405, i.e., based on the configuration generated in process 405. This constrains the articulatable segments within the boundary region as endoscope 100 moves further into the patient.
SEND COMMAND process 406 transfers to DEVICE MOTION PAUSED check operation 410 and to RETRACTION AND LINK RETRACTED check operation 407. RETRACTION AND LINK RETRACTED check operation 407 determines whether endoscope 100 is being retracted and whether a link has been retracted. If both of these conditions are true, check operation 407 transfers to DELETE WAYPOINT operation 408.
DELETE WAYPOINT operation 408 deletes the stored waypoint for the withdrawn link and transfers to operation 405. Hence, operations 405 to 408 cause endoscope 100 to follow safe path 150 as endoscope 100 is retracted from the patient.
In the example of FIG. 2, in response to the command, endoscope controller 240 configures the segments so that as the segments move forward, the segments are constrained to stay within the boundary region. Thus, the segments may not follow the trajectory of the tip exactly, but the segments follow a path within the boundary region that is safe.
When the operator starts to withdraw the endoscope from the patient, the waypoints in the stored ordered sequence of locations {P_k}_k=1 ^Ndecrease by one as each link is withdrawn. Processes 405 and 406 are used as steerable endoscope is withdrawn to maintain endoscope 100 with the boundary region.
As a further example of process 405, the control of the joint angles at constant endoscopic length increments is considered such that the endoscope configuration is kept within a safety area, the boundary region described above, during the process of insertion and withdrawal. In this example, a configuration of the trailing links that is closest, as measured by Euclidean norm, to the tip trajectory is generated in process 405, since the tip trajectory represents a safe route selected by the operator.
As described above P_kdenotes the Cartesian coordinate of the endoscope tip when the number of the inserted links is k. After the endoscope has been inserted for N links, the path trajectory of the endoscope tip is recorded in a series {P_k}_k=1 ^N. Process 405 controls the joint angles to achieve an endoscope link configuration close to the waypoints in recorded series {P_k}_k=1 ^N. This is formulated as the following optimization, where J is the cost function discussed above:
$\begin{matrix} \min_{θ} J = \frac{1}{2} \sum_{k = 1}^{N} { p_{k} - P_{k} }^{2} & (1) \end{matrix}$
subject to
θ_min≦θ≦θ_max, (2)
where p_kis the coordinate of each link determined by the kinematics model.
To apply an iterative procedure to solve this problem, assume that at the n-th step, a solution is θ_nand the next increment Δθ_nis to be determined. The new link coordinate is approximated by
p ⁿ⁺¹ ≈p ⁿ+∇_θ _n p·Δθ _n. (3)
Substituting expression (3) into the cost function gives:
$\begin{matrix} J^{n + 1} = \frac{1}{2} { p^{n + 1} - P }^{2} \approx \frac{1}{2} { \nabla_{θ_{n}} p \cdot Δ θ_{n} + p^{n} - P }^{2} . & (4) \end{matrix}$
This is indeed a quadratic programming problem:
$\begin{matrix} \min_{Δ θ} J = \frac{1}{2} Δ {θ_{n} (\nabla_{θ_{n}} p)}^{T} \nabla θ_{n} p Δ θ_{n} + {(p^{n} - P)}^{T} \nabla_{θ_{n}} p Δ θ_{n}, & (5) \end{matrix}$
subject to
θ_min−θ_n≦Δθ_n≦θ_max−θ_n. (6)

Kinematics Model

With respect to FIG. 5, let the first input parameter be defined as:
$\begin{matrix} q = [\begin{matrix} θ_{x}^{1} & θ_{y}^{} \\ θ_{x}^{2} & θ_{y}^{} \\ θ_{x}^{3} & θ_{y}^{} \\ θ_{x}^{4} & θ_{y}^{4} \end{matrix}] . & (7) \end{matrix}$
The vector of joint angles is defined as:
$\begin{matrix} q_{jw} = [\begin{matrix} \frac{θ_{x}^{1}}{4}, \frac{θ_{y}^{1}}{4}, \frac{θ_{x}^{1}}{4}, \frac{θ_{y}^{1}}{4}, \frac{θ_{x}^{1}}{4}, \frac{θ_{y}^{1}}{4}, \frac{θ_{x}^{1}}{4}, \frac{θ_{y}^{1}}{4}, \frac{θ_{x}^{2}}{4}, \frac{θ_{y}^{2}}{4}, \frac{θ_{x}^{2}}{4}, \frac{θ_{y}^{2}}{4}, \\ \frac{θ_{x}^{2}}{4}, \frac{θ_{y}^{2}}{4}, \frac{θ_{x}^{2}}{4}, \frac{θ_{y}^{2}}{4}, \frac{θ_{x}^{3}}{4}, \frac{θ_{y}^{3}}{4}, \frac{θ_{x}^{3}}{4}, \frac{θ_{y}^{3}}{4}, \frac{θ_{x}^{3}}{4}, \frac{θ_{y}^{3}}{4}, \frac{θ_{x}^{3}}{4}, \\ \frac{θ_{y}^{3}}{4}, \frac{θ_{x}^{4}}{4}, \frac{θ_{y}^{4}}{4}, \frac{θ_{x}^{4}}{4}, \frac{θ_{y}^{4}}{4}, \frac{θ_{x}^{4}}{4}, \frac{θ_{y}^{4}}{4}, \frac{θ_{x}^{4}}{4}, \frac{θ_{y}^{4}}{4} \end{matrix}], & (8) \end{matrix}$
and the state of each link is defined as:
$\begin{matrix} T_{1 w} (1) = [\begin{matrix} R_{y} (- \frac{θ_{x}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (1) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (3) = [\begin{matrix} R_{y} (- \frac{θ_{x}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (3) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (5) = [\begin{matrix} R_{y} (- \frac{θ_{x}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (5) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (7) = [\begin{matrix} R_{y} (- \frac{θ_{x}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (7) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (2) = [\begin{matrix} R_{x} (\frac{θ_{y}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (2) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (4) = [\begin{matrix} R_{x} (\frac{θ_{y}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (4) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (6) = [\begin{matrix} R_{x} (\frac{θ_{y}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (6) \end{matrix}) \\ 0 & 1 \end{matrix}], T_{1 w} (8) = [\begin{matrix} R_{x} (\frac{θ_{y}^{1}}{4}) & (\begin{matrix} 0 \\ 0 \\ - a (8) \end{matrix}) \\ 0 & 1 \end{matrix}], where R_{x} (θ) = [\begin{matrix} 1 & 0 & 0 \\ 0 & \cos (θ) & - \sin (θ) \\ 0 & \sin (θ) & \cos (θ) \end{matrix}] and R_{y} (- θ) = [\begin{matrix} \cos (θ) & 0 & - \sin (θ) \\ 0 & 1 & 0 \\ \sin (θ) & 0 & \cos (θ) \end{matrix}] & (9) \end{matrix}$
The rest of the T_lw's can be obtained in a similar fashion. In particular,
$\begin{matrix} T_{1 w} (33) = [\begin{matrix} I & (\begin{matrix} 0 \\ 0 \\ - a (33) \end{matrix}) \\ 0 & 1 \end{matrix}] . & (10) \end{matrix}$
Notice that all these T_lw's are in the Special Euclidean Lie group SE(4).
The cumulative transform to the base from the distal end of link k can be derived as
$\begin{matrix} T_{c 1} (k) = \prod_{j = 1}^{k} T_{1 w} (j) = T_{1 w} (1) \times T_{1 w} (2) \times \dots \times T_{1 w} (k) . & (11) \end{matrix}$
In this section, gradient ∇_θl_kis derived, where l is the position vector for link k. To this end, define the basis for the skew-symmetric matrix so(3) as follows:
$\begin{matrix} σ_{x} = [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & - 1 \\ 0 & 1 & 0 \end{matrix}], σ_{y} = [\begin{matrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ - 1 & 0 & 0 \end{matrix}], σ_{z} = [\begin{matrix} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] . & (12) \end{matrix}$
The rotation matrices can then be described as
R _x(θ)=exp(σ_xθ), R _y(θ)=exp(σ_yθ), R _z(θ)=exp(σ₂θ). (13)
Suppose that R is a rotation matrix
$\begin{matrix} R = [\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}] . & (14) \end{matrix}$
Since RR^T=I, an important property for R can be derived:
Rσ _x R ^T =r ₁₁σ_x +r ₂₁σ_y +r ₃₁σ_z, (15)
R ^Tσ_x R=r ₁₁σ_x +r ₁₂σ_y +r ₁₃σ_z, (16)
Rσ _y R ^T =r ₁₂σ_x +r ₂₂σ_y +r ₃₂σ_z, (17)
R ^Tσ_y R=r ₂₁σ_x +r ₂₂σ_y +r ₂₃σ_z, (18)
Now, the gradient dT_cl(k)/dθ is derived. Let
$\begin{matrix} T_{lw} (k) = [\begin{matrix} R_{k} & a_{k} \\ 0 & 1 \end{matrix}] and T_{c l} (k) = [\begin{matrix} S_{k} & l_{k} \\ 0 & 1 \end{matrix}] . & (19) \end{matrix}$
It is easy to derive that
$\begin{matrix} S_{k} = R_{1} R_{2} \dots R_{k}, & (20) \\ \begin{matrix} l_{k} = R_{1} R_{2} \dots R_{k - 1} a_{k} + R_{1} R_{2} \dots R_{k - 2} a_{k - 1} + \dots + R_{1} a_{2} + a_{1} \\ = S_{k - 1} a_{k} + S_{k - 2} a_{k - 1} + \dots + S_{1} a_{2} + a_{1}, \end{matrix} & (21) \end{matrix}$
and l₁=a₁. We therefore have
$\begin{matrix} \frac{\partial S_{k}}{\partial θ} = \frac{\partial}{\partial θ} (R_{1} R_{2} \dots R_{k}), & (22) \\ \frac{\partial l_{k}}{\partial θ} = \frac{\partial S_{k - 1}}{\partial θ} a_{k} + \frac{\partial S_{k - 2}}{\partial θ} a_{k - 1} + \dots + \frac{\partial S_{1}}{\partial θ} a_{2}, (k \geq 2) . & (23) \end{matrix}$
When θ=θ_x ¹, for k≧7, we have
$\begin{matrix} \begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} S_{k}^{T} = \frac{\partial}{\partial θ_{x}^{1}} (R_{1} R_{2} \dots R_{k}) S_{k}^{T} \\ = \frac{\partial R_{1}}{\partial θ_{x}^{1}} (R_{2} \dots R_{k}) S_{k}^{T} + R_{1} R_{2} \frac{\partial R_{3}}{\partial θ_{x}^{1}} (R_{4} \dots R_{k}) S_{k}^{T} + \\ R_{1} R_{2} R_{3} R_{4} \frac{\partial R_{5}}{\partial θ_{x}^{1}} (R_{6} \dots R_{k}) S_{k}^{T} + \\ R_{1} R_{2} R_{3} R_{4} R_{5} R_{6} \frac{\partial R_{7}}{\partial θ_{x}^{1}} (R_{8} \dots R_{k}) S_{k}^{T} \end{matrix} & (24) \\ = (- \frac{σ_{y}}{4}) + R_{1} R_{2} (- \frac{σ_{y}}{4}) {(R_{1} R_{2})}^{T} + R_{1} R_{2} R_{3} R_{4} (- \frac{σ_{y}}{4}) {(R_{1} R_{2} R_{3} R_{4})}^{T} + R_{1} R_{2} R_{3} R_{4} R_{5} R_{6} (- \frac{σ_{y}}{4}) {(R_{1} R_{2} R_{3} R_{4} R_{5} R_{6})}^{T} & (25) \\ = - \frac{σ_{y}}{4} + S_{2} (- \frac{σ_{y}}{4}) S_{2}^{T} + S_{4} (- \frac{σ_{y}}{4}) S_{4}^{T} + S_{6} (- \frac{σ_{y}}{4}) S_{6}^{T}, & (26) \end{matrix}$
which in turn yields
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} = - \frac{1}{4} (σ^{y} + S_{2} σ_{y} S_{2}^{T} + S_{4} σ_{y} S_{4}^{T} + S_{6} σ_{y} S_{6}^{T}) S_{k} . & (27) \end{matrix}$
For k=5 or 6, we obtain
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} S_{k}^{T} = - \frac{σ_{y}}{4} + S_{2} (- \frac{σ_{y}}{4}) S_{2}^{T} + S_{4} (- \frac{σ_{y}}{4}) S_{4}^{T}, thus \frac{\partial S_{k}}{\partial θ_{x}^{1}} = - \frac{1}{4} (σ_{y} + S_{2} σ_{y} S_{2}^{T} + S_{4} σ_{y} S_{4}^{T}) S_{k}; & (28) \end{matrix}$
for k=3 or 4,
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} = - \frac{1}{4} (σ_{y} + S_{2} σ_{y} S_{2}^{T}) S_{k}; & (29) \end{matrix}$
and for k=1 or 2,
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} = - \frac{1}{4} σ_{y} S_{k}; & (30) \end{matrix}$
In summary,
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{x}^{1}} = {\begin{matrix} - \frac{1}{4} σ_{y} S_{k}, & k = 1, 2; \\ - \frac{1}{4} (σ_{y} + S_{2} σ_{y} S_{2}^{T}) S_{k}, & k = 3, 4; \\ - \frac{1}{4} (σ_{y} + S_{2} σ_{y} S_{2}^{T} + S_{4} σ_{y} S_{4}^{T}) S_{k}, & k = 5, 6; \\ - \frac{1}{4} (σ_{y} + S_{2} σ_{y} S_{2}^{T} + S_{4} σ_{y} S_{4}^{T} + S_{6} σ_{y} S_{6}^{T}) S_{k}, & k \geq 7. \end{matrix} & (31) \\ \frac{\partial S_{k}}{\partial θ_{x}^{2}} = {\begin{matrix} 0, & k \leq 8; \\ - \frac{1}{4} (S_{8} σ_{y} S_{8}^{T}) S_{k}, & k = 9, 10; \\ - \frac{1}{4} (S_{8} σ_{y} S_{8}^{T} + S_{10} σ_{y} S_{10}^{T}) S_{k}, & k = 11, 12; \\ - \frac{1}{4} (S_{8} σ_{y} S_{8}^{T} + S_{10} σ_{y} S_{10}^{T} + S_{12} σ_{y} S_{12}^{T}) S_{k}, & k = 13, 14; \\ - \frac{1}{4} (S_{8} σ_{y} S_{8}^{T} + S_{10} σ_{y} S_{10}^{T} + S_{12} σ_{y} S_{12}^{T} + S_{14} σ_{y} S_{14}^{T}) S_{k}, & k \geq 15. \end{matrix} & (32) \\ \frac{\partial S_{k}}{\partial θ_{x}^{3}} = {\begin{matrix} 0, & k \leq 16; \\ - \frac{1}{4} (S_{16} σ_{y} S_{16}^{T}) S_{k}, & k = 17, 18; \\ - \frac{1}{4} (S_{16} σ_{y} S_{16}^{T} + S_{18} σ_{y} S_{18}^{T}) S_{k}, & k = 19, 20; \\ - \frac{1}{4} (S_{16} σ_{y} S_{16}^{T} + S_{18} σ_{y} S_{18}^{T} + S_{20} σ_{y} S_{20}^{T}) S_{k}, & k = 21, 22; \\ - \frac{1}{4} (S_{16} σ_{y} S_{16}^{T} + S_{18} σ_{y} S_{18}^{T} + S_{20} σ_{y} S_{20}^{T} + S_{22} σ_{y} S_{22}^{T}) S_{k}, & k \geq 23. \end{matrix} & (33) \end{matrix}$
Similarly,
$\begin{matrix} \frac{\partial S_{k}}{\partial θ_{y}^{1}} = {\begin{matrix} 0, & k = 1; \\ \frac{1}{4} (S_{1} σ_{x} S_{1}^{T}) S_{k}, & k = 2, 3; \\ \frac{1}{4} (S_{1} σ_{x} S_{1}^{T} + S_{3} σ_{x} S_{3}^{T}) S_{k}, & k = 4, 5; \\ \frac{1}{4} (S_{1} σ_{x} S_{1}^{T} + S_{3} σ_{x} S_{3}^{T} + S_{5} σ_{x} S_{5}^{T}) S_{k}, & k = 6, 7; \\ \frac{1}{4} (S_{1} σ_{x} S_{1}^{T} + S_{3} σ_{x} S_{3}^{T} + S_{5} σ_{x} S_{5}^{T} + S_{7} σ_{x} S_{7}^{T}) S_{k}, & k \geq 8. \end{matrix} & (34) \\ \frac{\partial S_{k}}{\partial θ_{y}^{2}} = {\begin{matrix} 0, & k \leq 9; \\ \frac{1}{4} (S_{9} σ_{x} S_{9}^{T}) S_{k}, & k = 10, 11; \\ \frac{1}{4} (S_{9} σ_{x} S_{9}^{T} + S_{11} σ_{x} S_{11}^{T}) S_{k}, & k = 12, 13; \\ \frac{1}{4} (S_{9} σ_{x} S_{9}^{T} + S_{11} σ_{x} S_{11}^{T} + S_{13} σ_{x} S_{13}^{T}) S_{k}, & k = 14, 15; \\ \frac{1}{4} (S_{9} σ_{x} S_{9}^{T} + S_{11} σ_{x} S_{11}^{T} + S_{13} σ_{x} S_{13}^{T} + S_{15} σ_{x} S_{15}^{T}) S_{k}, & k \geq 16. \end{matrix} & (35) \\ \frac{\partial S_{k}}{\partial θ_{y}^{3}} = {\begin{matrix} 0, & k \leq 17; \\ - \frac{1}{4} (S_{17} σ_{x} S_{17}^{T}) S_{k}, & k = 18, 19; \\ - \frac{1}{4} (S_{17} σ_{x} S_{17}^{T} + S_{19} σ_{x} S_{19}^{T}) S_{k}, & k = 20, 21; \\ - \frac{1}{4} (S_{17} σ_{x} S_{17}^{T} + S_{19} σ_{x} S_{19}^{T} + S_{21} σ_{x} S_{21}^{T}) S_{k}, & k = 22, 23; \\ - \frac{1}{4} (S_{17} σ_{x} S_{17}^{T} + S_{19} σ_{x} S_{19}^{T} + S_{21} σ_{x} S_{21}^{T} + S_{23} σ_{x} S_{23}^{T}) S_{k}, & k \geq 24. \end{matrix} & (36) \end{matrix}$
In this subsection, the Jacobian is derived. Let
$\begin{matrix} T_{J} (k) = \prod_{j = k + 1}^{N} T_{lw} (j) = T_{lw} (k + 1) \times \dots \times T_{lw} (N), & (37) \end{matrix}$
and denote its blocks as
$\begin{matrix} T_{J} (k) = [\begin{matrix} R_{k}^{J} & l_{k J}^{} \\ 0 & 1 \end{matrix}] . & (38) \end{matrix}$
Recalling Eq. (19), we have
$\begin{matrix} \begin{matrix} \frac{\partial T_{c l} (N)}{\partial θ_{x}^{1}} = [\begin{matrix} \frac{\partial S_{N}}{\partial θ_{x}^{1}} & \frac{\partial l_{N}}{\partial θ_{x}^{1}} \\ 0 & 0 \end{matrix}] \\ = \frac{\partial T_{lw} (1)}{\partial θ_{x}^{1}} T_{lw} (2) \dots T_{lw} (N) + \\ T_{lw} (1) T_{lw} (2) \frac{\partial T_{lw} (3)}{\partial θ_{x}^{1}} T_{lw} (4) \dots T_{lw} (N) + \\ T_{lw} (1) T_{lw} (2) T_{lw} (3) T_{lw} (4) \frac{\partial T_{lw} (5)}{\partial θ_{x}^{1}} T_{lw} (6) \dots \\ T_{lw} (N) + T_{lw} (1) T_{lw} (2) T_{lw} (3) T_{lw} (4) T_{lw} (5) T_{lw} (6) \\ \frac{\partial T_{lw} (7)}{\partial θ_{x}^{1}} T_{lw} (8) \dots T_{lw} (N) \\ = [\begin{matrix} - R_{1} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] T_{J} (1) + T_{c l} (2) [\begin{matrix} - R_{3} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] T_{J} (3) + \\ T_{c l} (4) [\begin{matrix} - R_{5} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] T_{J} (5) + T_{c l} (6) [\begin{matrix} - R_{7} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] T_{J} (7) . \end{matrix} & (39) \end{matrix}$
The last equality is obtained by the definition of T_cland T_J. Proceeding further, we have
$\begin{matrix} \frac{\partial T_{c l} (N)}{\partial θ_{x}^{1}} = [\begin{matrix} - R_{1} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] [\begin{matrix} R_{1}^{J} & l_{1}^{J} \\ 0 & 1 \end{matrix}] + \\ [\begin{matrix} S_{2} & l_{2} \\ 0 & 1 \end{matrix}] [\begin{matrix} - R_{3} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] [\begin{matrix} R_{3}^{J} & l_{3}^{J} \\ 0 & 1 \end{matrix}] + \\ [\begin{matrix} S_{4} & l_{4} \\ 0 & 1 \end{matrix}] [\begin{matrix} - R_{5} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] [\begin{matrix} R_{5}^{J} & l_{5}^{J} \\ 0 & 1 \end{matrix}] + \\ [\begin{matrix} S_{6} & l_{6} \\ 0 & 1 \end{matrix}] [\begin{matrix} - R_{7} \frac{σ_{y}}{4} & 0 \\ 0 & 0 \end{matrix}] [\begin{matrix} R_{7}^{J} & l_{7}^{J} \\ 0 & 1 \end{matrix}] \\ = [\begin{matrix} - S_{1} \frac{σ_{y}}{4} R_{1}^{J} - S_{3} \frac{σ_{y}}{4} R_{3}^{J} - S_{5} \frac{σ_{y}}{4} R_{5}^{J} - S_{7} \frac{σ_{y}}{4} R_{7}^{J} \\ 0 \\ - S_{1} \frac{σ_{y}}{4} l_{1}^{J} - S_{3} \frac{σ_{y}}{4} l_{3}^{J} - S_{5} \frac{σ_{y}}{4} l_{5}^{J} - S_{7} \frac{σ_{y}}{4} l_{7}^{J} \\ 0 \end{matrix}] . \end{matrix}$
Therefore, the derivative of the position vector l_Nwith respect to the joint angle θ_x ¹is
$\begin{matrix} \begin{matrix} \frac{\partial l_{N}}{\partial θ_{x}^{1}} = - S_{1} \frac{σ_{y}}{4} l_{1}^{J} - S_{3} \frac{σ_{y}}{4} l_{3}^{J} - S_{5} \frac{σ_{y}}{4} l_{5}^{J} - S_{7} \frac{σ_{y}}{4} l_{7}^{J} \\ = \frac{S_{1}}{4} [\begin{matrix} - l_{1, 3}^{J} \\ 0 \\ l_{1, 1}^{J} \end{matrix}] + \frac{S_{3}}{4} [\begin{matrix} - l_{3, 3}^{J} \\ 0 \\ l_{3, 1}^{J} \end{matrix}] + \frac{S_{5}}{4} [\begin{matrix} - l_{5, 3}^{J} \\ 0 \\ l_{5, 1}^{J} \end{matrix}] + \frac{S_{7}}{4} [\begin{matrix} - l_{7, 3}^{J} \\ 0 \\ l_{7, 1}^{J} \end{matrix}] . \end{matrix} & (40) \end{matrix}$
In the above examples, the sequence of ordered locations and six-degrees of freedom were considered in configuring articulatable segments to remain within a boundary region, sometimes called a safe region. However, in some applications, consideration of only two-degrees of freedom may be sufficient. The x-y position could be controlled, or the pitch and the yaw could be controlled. In these applications, each waypoint only includes two degrees of freedom corresponding to an insertion depth and the above processes become less computationally intensive while the safe path is followed.
System controller 220 (FIG. 2) is illustrated as unified structures for ease of illustration and understanding. This is illustrative only and is not intended to be limiting. The various component of system controller 220 can be located apart and still perform the functions described.
The above description and the accompanying drawings that illustrate aspects and embodiments of the present inventions should not be taken as limiting—the claims define the protected inventions. Various mechanical, compositional, structural, electrical, and operational changes may be made without departing from the spirit and scope of this description and the claims. In some instances, well-known circuits, structures, and techniques have not been shown or described in detail to avoid obscuring the invention.
While the memory in FIG. 2 is illustrated as a unified structure, this should not be interpreted as requiring that all memory is at the same physical location. All or part of the memory can be in a different physical location than a processor. Memory refers to a volatile memory, a non-volatile memory, or any combination of the two.
A processor is coupled to a memory containing instructions executed by the processor. This could be accomplished within a computer system, or alternatively via a connection to another computer via modems and analog lines, or digital interfaces and a digital carrier line.
All examples and illustrative references are non-limiting and should not be used to limit the claims to specific implementations and embodiments described herein and their equivalents. The headings are solely for formatting and should not be used to limit the subject matter in any way, because text under one heading may cross reference or apply to text under one or more headings. Finally, in view of this disclosure, particular features described in relation to one aspect or embodiment may be applied to other disclosed aspects or embodiments of the invention, even though not specifically shown in the drawings or described in the text.

Claims

1. A method comprising:

storing waypoints of a steerable medical device, wherein said stored waypoints comprise an ordered sequence of locations, and further wherein said ordered sequence of locations defines a safe path for moving an articulatable portion of said steerable medical device within a patient;

constraining said articulatable portion of said steerable medical device to follow said safe path as said articulatable portion moves within said patient.

2. The method of claim 1 wherein said constraining further comprises:

constraining said articulatable portion of said steerable medical device to remain within a boundary region enclosing said safe path as said articulatable portion of said steerable medical device follows said safe path.

3. The method of claim 2 wherein said boundary region comprises a tube having a cross-section.

4. The method of claim 3 wherein said cross-section comprises one of a circular cross-section, an oblate cross-section, and a rectangular cross cross-section.

5. The method of claim 2 wherein said articulatable portion is included in a plurality of segments of said steerable medical device.

6. The method of claim 5 wherein said constraining said articulatable portion of said steerable medical device to remain within a boundary region enclosing said safe path further comprises:

generating position and orientation data for each of said plurality of segments using said ordered sequence of locations and a kinematic model of said plurality of segments of said steerable medical device.

7. The method of claim 6 wherein said generating further comprises:

minimizing a cost function to generate said position and orientation data for each of said plurality of segments.

8. The method of claim 7 wherein said minimizing a cost function further comprises:

minimizing the sum of the absolute values of relative joint angles, with an additional constraint that link positions not deviate by more than a distance Δ at each of the waypoints in said ordered sequence of locations.

9. The method of claim 1 wherein said articulatable portion of said steerable medical device comprises a segment of said steerable medical device and said segment comprises a plurality of links.

10. An apparatus comprising:

a steerable medical device having an articulatable portion; and

a controller, coupled to said steerable medical device, comprising:

a processor;

a memory coupled to said processor; and

a medical device controller coupled to said processor and to said steerable medical device; and

wherein said processor stores in said memory an ordered sequence of locations; and

said processor analyzes said ordered sequence of locations and outputs to said medical device controller at least one command to constrain said articulatable portion within a boundary region enclosing a safe path defined by said ordered sequence of locations.

11. The apparatus of claim 10 wherein said articulatable portion of said steerable medical device comprises a segment of said steerable medical device and said segment comprises a plurality of links.

12. The apparatus of claim 10 wherein said boundary region comprises a tube having a cross-section.

13. The apparatus of claim 12 wherein said cross-section comprises one of a circular cross-section, an oblate cross-section, and a square cross cross-section.