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

A method comprising:

• storing, by a controller, waypoints of a steerable medical device, the waypoints being locations of the steerable medical device within a patient as the medical device is moved within the patient, the waypoints being determined as the steerable medical device is moved towards a desired location within the patient, 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 the patient, the articulatable portion of the steerable medical device comprising one or more segments, each of the one or more segments comprising a plurality of links, adjacent links of the plurality of links being connected by a joint;
• and
• constraining, by the controller as the articulatable portion of the steerable medical devices is moved within the patient, the articulatable portion of the steerable medical device to remain within a boundary region enclosing the safe path as the articulatable portion of the steerable medical device follows the safe path, the boundary region being based on the waypoints,
• wherein a link in the articulatable segment deviating from the safe path remains within the boundary region,
• wherein the constraining comprises:
• generating position and orientation data for each of the one or more segments of the articulatable portion using the ordered sequence of locations and a kinematic model of the articulatable portion of the steerable medical device;
• and
• wherein said generating further comprises:
• minimizing a cost function associated with the kinematic model to generate said position and orientation data for each of said one or more segments;
• and
• 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.