Patent Application
20140288416
Magnetic manipulation of capsule endoscopes has the potential to make current gastrointestinal screening procedures faster, safer, and less invasive. To date, three electromagnetic systems have been developed with the ability to perform five degree-of-freedom (5-DOF) manipulation of an untethered magnetic device, such as a magnetic capsule endoscope. The MAGNETECS and OCTOMAG systems consist of eight electromagnets arranged around a sphere and hemisphere, respectively, and directed toward the manipulation workspace. A system has been developed by Siemens, consisting of 12 electromagnets through which a patient is positioned, for the control of a capsule endoscope in a water-filled stomach. Permanent-magnet actuation systems are gaining attention for their ability to generate fields with clinically relevant strengths, inexpensively and in a compact form-factor, compared to electromagnetic systems.
Previous permanent-magnet systems for endoscopy have been limited to dragging and rolling capsule endoscope devices on the stomach's surface or large colon. For example, such permanent magnet systems cannot levitate a capsule in space, such as when moving the capsule, while also controlling the capsule's heading. Heading and, typically, translation have previously been done when the capsule was on a surface when utilizing permanent magnets or constant magnetic fields. Accordingly, improvements continue to be sought to avoid such drawbacks. The present technology provides magnetic 3-DOF position and 2-DOF orientation control of a stomach capsule endoscope in fluid, using a magnet providing a constant magnetic field. Manipulation of untethered devices with 5-DOF has been previously demonstrated with electromagnet systems with variable magnetic fields.
In one aspect, the present technology provides a method of manipulating a magnetic capsule endoscope or other untethered magnetic device in 3-D space when not in contact with a surface. The method can include positioning a magnet actuator and an untethered magnetic device proximate one another such that the untethered magnetic device is within a constant magnetic field of the magnet actuator. The method can also include determining a position of the untethered magnetic device relative to the magnet actuator. The method can further include identifying a desired heading, position, and/or velocity of the untethered magnetic device. Still further, the method can include calculating a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve the desired heading, position, and/or velocity. In addition, the method can include moving the magnetic actuator to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. The magnetic field heading can be operable to orient the untethered magnetic device to the desired heading (like a compass needle) and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the magnetic device levitates above a surface.
The present technology can also provide a computer implemented method of controlling a magnet actuator to manipulate an untethered magnetic device. The method can be performed under control of a processor and memory configured with executable instructions. The method can include identifying a relative position of an untethered magnetic device and a magnet actuator, the untethered magnetic device being within a constant magnetic field of the magnet actuator. The method can also include identifying a desired heading, position, and/or velocity of the untethered magnetic device. The method can further include calculating a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve the desired heading, position, and/or velocity. In addition, the method can include transmitting instructions to move the magnet actuator to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. The magnetic field heading can be operable to orient the untethered magnetic device to the desired heading and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the untethered magnetic device levitates above a surface.
In another aspect, the present technology provides a system for manipulating an untethered magnetic device. The system can include a magnet actuator configured to generate a constant magnetic field to influence a heading, a position, and/or a velocity of an untethered magnetic device. The system can also include a localization device to determine a position of the untethered magnetic device relative to the magnet actuator. The system can further include a control system to calculate a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve a desired device heading, position, and/or velocity of the untethered magnetic device. In addition, the system can include a movement device configured to move the magnetic actuator to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. The magnetic field heading can be operable to orient the untethered magnetic device in the desired heading and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the magnetic device levitates above a surface.
Additional variations and aspects of the invention can be appreciated from the following detailed description.
These figures are provided merely for convenience in describing specific embodiments of the invention. Alteration in dimension, materials, and the like, including substitution, elimination, or addition of components can also be made consistent with the following description and associated claims. Reference will now be made to the exemplary embodiments illustrated, and specific language will be used herein to describe the same. It will nevertheless be understood that no limitation of the scope of the invention is thereby intended.
Reference will now be made to certain examples, and specific language will be used herein to describe the same. Examples discussed herein set forth a system for manipulating an untethered magnetic device and associated methods that can orient and translate an untethered magnetic device with a constant magnetic field as the magnetic device levitates above a surface.
With the general embodiments set forth above, it is noted that when describing a system for manipulating an untethered magnetic device, or the related methods, each of these descriptions are considered applicable to the other, whether or not they are explicitly discussed in the context of that embodiment. For example, in discussing the system per se, the method embodiments are also included in such discussions, and vice versa.
It is to be understood that this invention is not limited to the particular structures, process steps, or materials disclosed herein, but is extended to equivalents thereof as would be recognized by those ordinarily skilled in the relevant arts. It should also be understood that terminology employed herein is used for the purpose of describing particular embodiments only and is not intended to be limiting.
It must be noted that, as used in this specification and the appended claims, the singular forms “a,” “an,” and “the” include plural referents unless the context clearly dictates otherwise. Thus, for example, reference to “a processor” includes one or more of such processors.
Also, it is noted that various modifications and combinations can be derived from the present disclosure and illustrations, and as such, the following figures should not be considered limiting.
In describing and claiming the present invention, the following terminology will be used in accordance with the definitions set forth below.
As used herein, the term “substantially” refers to the complete or nearly complete extent or degree of an action, characteristic, property, state, structure, item, or result. For example, an object that is “substantially” enclosed would mean that the object is either completely enclosed or nearly completely enclosed. The exact allowable degree of deviation from absolute completeness may in some cases depend on the specific context. However, generally speaking the nearness of completion will be so as to have the same overall result as if absolute and total completion were obtained. The use of “substantially” is equally applicable when used in a negative connotation to refer to the complete or near complete lack of an action, characteristic, property, state, structure, item, or result.
As used herein, “adjacent” refers to the proximity of two structures or elements. Particularly, elements that are identified as being “adjacent” may be either abutting or connected. Such elements may also be near or close to each other without necessarily contacting each other. The exact degree of proximity may in some cases depend on the specific context.
As used herein, “heading” denotes a pointing orientation of the device without regard to the device's rotation around the pointing axis.
As used herein, a plurality of items, structural elements, compositional elements, and/or materials may be presented in a common list for convenience. However, these lists should be construed as though each member of the list is individually identified as a separate and unique member. Thus, no individual member of such list should be construed as a de facto equivalent of any other member of the same list solely based on their presentation in a common group without indications to the contrary.
Concentrations, amounts, and other numerical data may be expressed or presented herein in a range format. It is to be understood that such a range format is used merely for convenience and brevity and thus should be interpreted flexibly to include not only the numerical values explicitly recited as the limits of the range, but also to include all the individual numerical values or sub-ranges encompassed within that range as if each numerical value and sub-range is explicitly recited. As an illustration, a numerical range of “about 1 to about 5” should be interpreted to include not only the explicitly recited values of about 1 to about 5, but also include individual values and sub-ranges within the indicated range. Thus, included in this numerical range are individual values such as 2, 3, and 4 and sub-ranges such as from 1-3, from 2-4, and from 3-5, etc., as well as 1, 2, 3, 4, and 5, individually. This same principle applies to ranges reciting only one numerical value as a minimum or a maximum. Furthermore, such an interpretation can apply regardless of the breadth of the range or the characteristics being described.
Any steps recited in any method or process claims may be executed in any order and are not limited to the order presented in the claims unless otherwise stated. Means-plus-function or step-plus-function limitations will only be employed where for a specific claim limitation all of the following conditions are present in that limitation: a) “means for” or “step for” is expressly recited; and b) a corresponding function is expressly recited. The structure, material or acts that support the means-plus function are expressly recited in the description herein. Accordingly, the scope of the invention should be determined solely by the appended claims and their legal equivalents, rather than by the descriptions and examples given herein.
One example of a system 100 for manipulating an untethered magnetic device 120 is illustrated in
The magnet actuator 110 can be moved and manipulated by movement device 130, such as a robotic manipulator. Thus, in one aspect, the magnet actuator can be considered an end effector for the robotic manipulator. The robotic manipulator can be configured to move the magnet actuator in multiple degrees of freedom (DOF). For example, as shown in the figure, the robotic manipulator can be configured to move in 5-DOF. In particular, the robotic manipulator illustrated is configured to provide rotational degrees of freedom for various segments or linkage arms of the robotic manipulator. In this case, the robotic manipulator is configured to rotate a first linkage arm 131 about a first axis 101, a second linkage arm 132 about a second axis 102, a third linkage arm 133 about an axis 103, a fourth linkage arm 134 about an axis 104, and a fifth linkage arm 135 about an axis 105. The magnet actuator can be coupled to the fifth linkage arm, thus being movable in the 5-DOF provided by the robotic manipulator to control the untethered magnetic device, as disclosed in more detail hereinafter.
With continued reference to
As illustrated in the figure, the magnet actuator can comprise an axially magnetized magnet 111, which has a dipole moment ma. The magnet actuator can include only a single permanent magnet or multiple permanent magnets having a composite magnetic dipole. In one aspect, the magnet actuator can comprise an electromagnet having a constant magnetic field when energized or “on,” as opposed to a varying magnetic field when energized, which is often the case with electromagnets.
In one optional aspect, the magnet actuator 110 can include one or more secondary electromagnets 112a, 112b, in addition to the primary magnet 111. In this case, the primary magnet can provide “coarse” control of the untethered magnetic device 120 and the secondary electromagnets can provide “fine” corrections to the coarse control of the permanent magnet. Thus, the secondary electromagnets can provide a relatively small magnetic force to cause a small amount of fine correction to the heading, position, and/or velocity of the untethered magnetic device, instead of having to move the robotic arm to achieve the same result. In another aspect, the secondary electromagnets can be positioned nearby the actuator magnet on a stationary platform near the untethered device sufficient to affect movement of the untethered device.
With continued reference to
The dipole moments ma and mc of the magnet actuator and the untethered magnetic device, respectively, are discussed in more detail hereinafter with regard to a method of manipulating the untethered magnetic device.
With reference to
In addition, the system 100 can include a control system 150 for controlling the untethered magnetic device 120 with the magnet actuator 110. The control system can include a processor 151 and memory 152 to calculate a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve a desired heading, position, and/or velocity of the untethered magnetic device. The desired heading, position, and/or velocity of the untethered magnetic device can be provided by a human user and/or by the processing system.
In use, the localization device 140 can determine a position of the untethered magnetic device 120, which can be used to identify the relative position of the magnet actuator 110 and the untethered magnetic device. A user can then input, such as via a keyboard, mouse, joystick, controller, etc., a desired heading, position, and/or velocity of the untethered magnetic device. The control system can then calculate a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve a desired heading, position, and/or velocity of the untethered magnetic device. With this information, the movement device 130, such as the robotic manipulator, can move the magnetic actuator, as appropriate, to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. In contrast to other manipulation systems utilizing a constant magnetic field, the magnetic field heading can be operable to orient the untethered magnetic device in the desired heading and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the magnetic device levitates above a surface. In other words, the untethered magnetic device can be caused to change heading and move while levitating or being suspended against gravity without solid physical contact. As described in more detail below, this kind of control of the untethered magnetic device can be achieved by decoupling calculations of the magnetic field heading and the magnetic force. Thus, although inherently coupled in a physical sense, the magnetic field heading, which provides the desired device heading, can be modeled and calculated independent of the magnetic force, which provides the desired position and/or velocity of the untethered magnetic device.
In one aspect of the present disclosure, a method of manipulating the untethered magnetic device is disclosed, which can be utilized by the system 100 disclosed hereinabove. The untethered magnetic device, referred to as a capsule hereinafter, is assumed to contain a magnet with a constant magnetic field, such as a permanent magnet, positioned at the capsule's center-of-gravity, with its dipole moment (i.e., the vector from the south to north pole) denoted by mcε3 in units A·m2, which is assumed to be parallel to the capsule's principle axis. The actuator magnet's dipole moment is denoted by maε
3 and is positioned by a robotic manipulator with at least 5-DOF; rotation of the actuator magnet about ma is not needed. The positions of the actuator and capsule endoscope magnet centers are denoted by paε
3 and pcε
3, respectively, in units m.
It is assumed that the magnetic field h(p, {circumflex over (m)}a)ε3 generated by the actuator magnet can be modeled by the point-dipole model, which is given by (1)
where p=pc−pa is the vector from the center of the actuator magnet to the center of the capsule's magnet (i.e., the relative position), D({circumflex over (p)})=3{circumflex over (p)}{circumflex over (p)}T−I, and Iε3×3 is the identity matrix. Since the magnitudes of ma and mc are constant (they are the dipole moments of permanent magnets), all functions of ma and mc are expressed as functions of {circumflex over (m)}a and {circumflex over (m)}c to explicitly indicate that their magnitudes do not vary. Equation (1) exactly predicts the field produced by a spherical magnet and is an approximation for every other geometry that becomes more accurate with increasing distance. The geometry of a nonspherical magnet can be adjusted to make equation (1) a more accurate approximation in the near-field. It is assumed that the dipole field accurately models the field of the actuator magnet.
The robot manipulator's n revolute and prismatic joint velocities {dot over (q)}εn are mapped to the actuator magnet's spatial {dot over (p)}a and angular ωa velocities by the robot manipulator Jacobian matrix JR(q)ε
6×n as given by (2):
The point-dipole field equation (1) is radially symmetric about the actuator dipole moment and any component of ωa in the direction of {circumflex over (m)}a produces no change in the magnetic field applied to the capsule. As a result, the robot manipulator Jacobian JR(q) can be converted into an actuator-magnet Jacobian matrix JA(q) that maps manipulator joint velocity {dot over (q)} to the actuator magnet's spatial velocity {dot over (p)}a and the actuator dipole moment's directional velocity {dot over ({circumflex over (m)}a=ωa×{circumflex over (m)}a, with no contribution from the component of ωa parallel to {circumflex over (m)}a, by (3)
where S({circumflex over (m)}a)εso(3) is the skew-symmetric form of the cross-product operation. The matrix JA(q) can be used to approximately map small changes in the manipulator's joints to small changes in actuator-magnet position and small changes in the heading of the actuator magnet's dipole moment, as given by (4):
Note that JA(q) is not invertible and is at most rank five.
When the capsule is placed in the magnetic dipole field (1) generated by the actuator magnet, a magnetic torque Tm=μ0mc×h(p, {circumflex over (m)}a) and force fm=μ0(mc·Δ)h(p, {circumflex over (m)}a) are applied to the capsule's magnet, which are given by (5) and (6), respectively,
where Z=I−5{circumflex over (p)}{circumflex over (p)}T and μ0=4π×10−7\,N·A−2 is the permeability of free-space. The magnetic torque aligns the capsule's dipole moment with the applied field, while the magnetic force pulls the capsule in a direction determined by the field's spatial derivatives and the capsule's dipole moment.
When the magnetic capsule is actuated in fluid at low speeds, small accelerations, and without contact with other objects, there is little resistance to change in the capsule's heading, which enables the magnetic torque to quickly align the capsule's dipole moment with the applied field. In these conditions, it is assumed that the capsule's dipole moment is approximately aligned with the applied field for all time (7):
{circumflex over (m)}
c(p,{circumflex over (m)}a)≈ĥ(p,{circumflex over (m)}a)= (7)
and the capsule's heading can be controlled by adjusting the direction of the magnetic field without controlling the magnetic torque directly using (5), which would require measurement of the direction of {circumflex over (m)}c (i.e., the capsule's heading). It also implies that {circumflex over (m)}c can be predicted by (7) using only a measurement of the position p obtained by a localization system, and that the magnetic force applied to the magnetic capsule can be predicted by substituting (7) into (6) to get (8):
The total force f applied to the capsule consists of the apparent weight fw (sum of the capsule's weight and buoyant force), which is constant, and the magnetic force fm. It is assumed that the capsule is heavier than its buoyant force, making fw point in the direction of gravity. In this case, the capsule can be made to levitate by positioning the actuator magnet above the capsule, where the attractive magnetic force perfectly balances the capsule's apparent weight and the magnitude of the total applied force f is zero. If the capsule is desired to ascend, then the actuator magnet is moved closer so that the magnetic force is larger than the capsule's apparent weight and the total applied force is directed upward. If the capsule is desired to descend, then the actuator magnet is positioned farther away from the capsule's levitation position and the total applied force points down. The maximum downward force that can be applied is the capsule's apparent weight fw.
Using (7) and (8), a nonlinear magnetic actuation equation (9) can be formed that relates the relative position p and the direction of the actuator magnet {circumflex over (m)}a to the total applied force f and the direction of the applied magnetic field ĥ:
which is purely a function of the actuator magnet's pose, that is, the relative position p and the actuator magnet's dipole moment direction {circumflex over (m)}a, which in turn, are purely specified by the capsule's position pc and the robot manipulator's pose q.
In order to solve the “inverse” problem (i.e., computing the necessary manipulator pose that will apply a desired total applied force and an applied magnetic field heading, given the capsule's position), the nonlinear actuation equation (9) is first linearized with the Jacobian matrix JFε6×6, computed by differentiating (9) with respect to the relative position p and the actuator dipole moment {circumflex over (m)}a. Linearization produces the approximate mapping between small changes in relative position and actuator moment direction to small changes in the applied force and field heading, as shown in (10) and (11):
where (11) results from substituting δp=δpc−δpa into (10). The relation (11) divides a small change in applied total force and applied field heading into the result of a small change in capsule position δpc and a small change in the actuator magnet's pose (i.e., the actuator-magnet position δpa and dipole heading δ{circumflex over (m)}a), which is related to small changes in the manipulator's joints by the Jacobian JA (4). Linearization can be one way to decouple calculations of the magnetic field heading and the magnetic force. Another alternative approach is an iterated explorative method that first translates the actuator magnet using equation 10 to change the applied force, and then using an estimate of the capsule's new position in space, using equation 9 to adjust the actuator magnet heading to achieve the desired capsule heading.
Substituting (4) into (11) produces the relationship between small changes in the manipulator's joints and capsule position to small changes in applied total force and field heading, as shown in (12) and (13):
The actuator magnet's dipole moment {circumflex over (m)}a does not appear in the arguments of JFA since {circumflex over (m)}a is set by the robot manipulator's joints q using the manipulator's forward kinematics.
Equation (12) is intended to be used inside a control loop where small changes in capsule position δpc are obtained by a capsule localization system, and δf and δĥ are small desired changes produced by a controller governing the magnetic capsule's pose. In this context, the terms of (12) can be rearranged to produce (14)
where δd is a desired change in applied force and field heading resulting only from a change in the manipulator's joints. Equation (14) can be inverted to produce the inverse mapping of desired change in applied force and a change in field heading to a necessary change in the manipulator's joints using the Moore-Penrose pseudoinverse, as in (15):
δq≈JFA(p,q)\δd. (15)
If multiple solutions of (15) are possible (i.e., the manipulator has more than 5-DOF), then the pseudoinverse solves (15) and minimizes |δq|. (A generalized pseudoinverse can be applied for a manipulator where the units of δq are inconsistent.) Given an initial joint configuration q0, (15) can be integrated in time to produce qt without explicitly solving the inverse kinematics of the complete manipulator-magnet system. This approach breaks down when the manipulator is near a kinematic singularity, which is address hereinafter.
For 5-DOF holonomic control, JFA(p, q) must be rank five. Since JFA(p, q) is the product of JF(p, {circumflex over (m)}a) and JA(q), we will analyze the rank of the Jacobian JF(p, {circumflex over (m)}a) and the Jacobian JA(q) separately. For readability, the Jacobians JFA(p, q), JF(p, {circumflex over (m)}a) and JA(q) are referred to without their arguments in the text (i.e., as JFA, JF, and JA).
Prior to analyzing the rank of JF, we first scale the columns and rows of JF to produce a nondimensional Jacobian {tilde over (J)}F that approximately maps its preimage, consisting of nondimensional changes in position δp/|p| and changes in actuator magnet heading δ{circumflex over (m)}a (already nondimensional), to its image, consisting of nondimensional changes in force δf/|fm| and applied field heading δĥ (already nondimensional), as in (16):
where Iε3×3 is the identity matrix.
The nondimensional Jacobian {tilde over (J)}F is produced by post- and premultiplying JF with a series of elementary matrices, which guarantees that rank {tilde over (J)}F=rank JF and enables the rank of JF to be found using the singular value decomposition of {tilde over (J)}F with unit-consistent singular values, which reveal the rank of JF. Since the applied field direction ĥ cannot change in a direction parallel to itself, the smallest singular value σ6 must be zero. The second smallest singular value σ5 reveals whether the rank of rank {tilde over (J)}F=5. The minimum value taken on by σ5 is 0.123, indicating that {tilde over (J)}F (and thus JF) is always rank five.
The fact that JF is always rank five implies that a single permanent magnet in space, irrespective to the robot manipulator that maneuvers it, can exhibit 5-DOF control over an untethered magnetic device. The ability of a complete robotic system, including magnet and manipulator, to exhibit 5-DOF magnetic control is precluded only by the ability of the robot manipulator to position the actuator magnet with 3-DOF and the actuator magnet's dipole moment with 2-DOF. If the rank of the Jacobian JA is five, then the robotic system possesses 5-DOF control over the untethered capsule. If the actuator-magnet pose required to achieve a desired applied total force and magnetic field heading places the manipulator into a kinematic singularity, then 5-DOF magnetic control is lost.
The configurations of total forces and field headings that make the manipulator enter a singularity are numerically analyzed by first nondimensionalizing the Jacobian JA as (17)
which can then be substituted, along with {tilde over (J)}F, into (13) for JF and JA to produce the normalized Jacobian (18)
which approximately maps change in manipulator joints δq (already nondimensional) to change in nondimensional applied force and change in field heading (already nondimensional).
The Moore-Penrose pseudoinverse {tilde over (J)}FA\ is the inverse mapping that minimizes |δq| if the robot manipulator is over-actuated. The largest singular value of {tilde over (J)}FA\ (i.e., the reciprocal of the smallest nonzero singular value of {tilde over (J)}FA) can be used to describe the worst case of how a unit-magnitude vector of nondimensional change in applied force and field heading are approximately mapped to a magnitude change in manipulator joints. If the largest singular value approaches infinity, then the robot manipulator is near a kinematic singularity.
As an example, illustrated in
{circumflex over (m)}
a=−{circumflex over (z)}, (19)
where D−1({circumflex over (p)})=(D({circumflex over (p)})−I)/2 admits a unique actuator magnet pose for every actuator magnet position.
The largest singular value of {tilde over (J)}FA\ resulting from the robot manipulator configuration that places the actuator magnet in every feasible position in the {circumflex over (x)},{circumflex over (z)} and ŷ,{circumflex over (z)} planes and directs the actuator magnet's moment according to (19) are shown in
Each actuator magnet pose causes a magnetic force to be applied to the capsule. Fifteen numbered actuator magnet poses are illustrated in
In one aspect, a manipulator's motion near kinematic singularities can be managed, while applying differential kinematic inversion. A strategy can be implemented that sacrifices control over the capsule's heading in order to maintain control over the magnetic force applied to the capsule (thus its position) in the presence of a manipulator singularity. In other words, the ability to control the untethered magnetic device's heading can be sacrificed to maintain control over the untethered magnetic device's position when a robotic manipulator moving the magnetic actuator enters a kinematic singularity. Sacrificing heading control transforms the complete magnetic manipulation system into one that is kinematically over-actuated.
Given a small desired change in applied field heading δĥd and a small desired change in applied magnetic force δfd, the problem of sacrificing heading control, while maintaining control over the applied magnetic force, is posed as a constrained, quadratic least-squares problem, of the form of (20), (21), and (22), respectively,
which is solved numerically, where the matrices ∂f/∂qε3×n and ∂ĥ/∂qε
3×n are the top and bottom three rows of the Jacobian JFA, respectively. The constraint (21) guarantees the desired change in applied force δfd is met (provided ∂f/∂q has full row rank), and the constraint (22) enforces a maximum bound r on the magnitude of joint motion, weighted by the invertible matrix W. The cost function (20) attempts to reduce the error between the desired and actual change in applied field heading. The weight matrix W can be used to penalize select joint motions, to homogenize disparate units of δq, or to keep the magnitude of δq within a “trust-region,” where the Jacobian JFA is accurate. Note that if the magnitude constraint (22) is inactive (e.g. if the robot manipulator is not near a kinematic singularity) and JFA is rank-five, then the solution to the formulation (20)-(22) is equivalent to the solution obtained with the pseudoinverse (15).
There are two ways for the formulation (20)-(22) to break down. The first is if the matrix ∂f/∂q does not have full row rank and the constraint (21) is not satisfiable. The second is if the constraints (21) and (22) become mutually exclusive, which could occur if ∂f/∂q is ill-conditioned, |δfd| is too large, or r is too small for the required joint motion δq.
In one aspect, the weight matrix can be selected with the robotic manipulator in mind. For example, if the matrix reduces the components that correspond to change in relative position p, then the resulting solution will favor large values of p, requiring large movements of the robot end-effector. In another aspect, the weights can be adjusted based on the observed behavior of the robot manipulator. This weighting scheme can be useful when no change in capsule heading or position is desired. The weight matrix can be constant, or it can change over time (e.g., as a function of the configuration of the robotic manipulator). If the weight matrix is a function of the manipulator's velocity Jacobian matrix that relates the actuator dipole moment and relative position p to changes in the robot joint angles, then the weight matrix could be made to penalize combinations of both the actuator dipole moment and p that produce the most change in joint configuration. Such a strategy could be implemented that forces the controller to reduce the joint motion of the robot.
In accordance with one example of the present disclosure, a method of manipulating an untethered magnetic device is provided. The method can include positioning a magnet actuator and an untethered magnetic device proximate one another such that the untethered magnetic device is within a constant magnetic field of the magnet actuator. The method can also include determining a position of the untethered magnetic device relative to the magnet actuator. The method can further include identifying a desired heading, position, and/or velocity of the untethered magnetic device. Still further, the method can include calculating a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve the desired heading, position, and/or velocity. In addition, the method can include moving the magnetic actuator to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. The magnetic field heading can be operable to orient the untethered magnetic device in the desired heading and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the magnetic device levitates above a surface.
In another example of the present disclosure, a computer implemented method of controlling a magnet actuator to manipulate an untethered magnetic device is provided. The method can be performed under control of a processor and memory configured with executable instructions. The method can include identifying a relative position of an untethered magnetic device and a magnet actuator, the untethered magnetic device being within a constant magnetic field of the magnet actuator. The method can also include identifying a desired heading, position, and/or velocity of the untethered magnetic device. The method can further include calculating a magnetic field heading and/or a magnetic force to be applied to the untethered magnetic device to achieve the desired device heading, position, and/or velocity. In addition, the method can include transmitting instructions to move the magnet actuator to apply the calculated magnetic field heading and/or magnetic force to the untethered magnetic device via the magnetic field. The magnetic field heading can be operable to orient the untethered magnetic device in the desired heading and the magnetic force can be operable to translate the untethered magnetic device to the desired position and/or at the desired velocity as the magnetic device levitates above a surface. It is noted that no specific order is required in the methods disclosed herein, though generally in one embodiment, the method steps can be carried out sequentially.
The methods disclosed herein involve 3-DOF localization of the capsules position, but does not require an estimate of the capsule's heading. In one aspect, the position of the magnetic capsule endoscope can be estimated by calculating an estimated solution based on an environment model. Typically, even physical measurement of position can be noisy. As such, approximation and estimators can be utilized (e.g., a Kalman filter) that uses knowledge of the devices dynamics to filter out noise in the sensor. The estimator can take in measurements as one of its inputs. The estimator can produce estimates of the capsule's position and optionally orientation in continuous space, or the estimator can act as a finite state machine (e.g., the capsule is either on the stomach floor or the capsule is at the fluid surface), or the estimator can produce finite states in some degrees of freedom and continuous estimates in other degrees of freedom. Thus, state-space estimators and Bayesian methods can be used.
A mockup capsule was actuated in a tank of water by an axially magnetized, grade N42, cylindrical NdFeB magnet with a height of 31.75 mm, a diameter of 31.75 mm, and with a dipole moment of |ma|=26.2 A·m2, positioned by a Yaskawa-Motoman MH5 6-DOF robotic manipulator. The capsule contained a cube NdFeB permanent magnet with its dipole moment |mc|=0.126 A·m2 arranged parallel to the capsule's principal axis. The remainder of the capsule's volume was filled with air. The capsule's weight was 15.3 mN and the buoyancy force in water was 14.8 mN. The position of the capsule was triangulated by two orthogonal Basler A602FC cameras, which were used with an extended Kalman filter for capsule-position feedback. Unless otherwise stated, the localization system's update frequency was 90 Hz. The experimental setup of the robot manipulator, vision system, mockup capsule, and the actuator magnet was similar to that shown in
A PID feedback controller (using the triangulated capsule position) with a gravity-compensating feed-forward term, was implemented to servo the capsule to any desired position in the workspace. At every iteration, the PID controller took as input a desired capsule position pc,d, an estimated capsule position 6×6 unless otherwise noted. An estimate of the capsule's heading was obtained from the measured position using (1), and was controlled in an open-loop fashion. A desired change in field heading (which is equivalent a desired change in capsule heading) was computed as the difference between a desired capsule heading and an estimate of the current capsule heading, which was assumed to be small at each controller iteration. Due to a limitation of the commercial manipulator control system, the robot manipulator's position was updated at 25 Hz.
The theory presented herein, as well as the control system, was demonstrated by controlling the magnetic mockup capsule along multiple predefined trajectories.
The vision system used to track the 3-DOF capsule position may not be feasible for clinical use. Existing clinically relevant localization strategies include RF triangulation, magnetic methods, and CT scan or x-ray fluoroscopy. In some experiments, the mockup capsule's position was localized at 90 Hz by the vision system. Clinically feasible localization methods may not provide the capsule's position at high rates (one known method can perform 3D position-tracking at approximately 50 Hz). The ability to actuate a mockup capsule with reduced 3D localization update frequencies is illustrated in
The mockup capsule was also transitioned from a configuration that forced the robot manipulator to enter its wrist singularity, to a configuration where the capsule pointed in the −{circumflex over (z)} direction by rotating the desired capsule direction 10° around the −ŷ axis, while simultaneously keeping the capsule's position in space stationary. In this example, the weight matrix W=diag([1, 1, 1, 20, 1, 1]) and the bound r=0.04 radians, which penalizes motion in the manipulator's fourth joint more than the others. In the initial robot manipulator configuration at time t=0 s, the desired change in the capsule direction would require the fourth joint of the robot manipulator to rapidly rotate π/2 radians. Rather than rapidly rotating the fourth joint, the constraint |Wδq|≦r penalized the fourth joint's velocity and forced the manipulator to first rotate the capsule about the {circumflex over (z)} axis before rotating about the ŷ axis as desired. This demonstrated the ability of the controller to balance desired changes in the capsule's configuration that may conflict with the robot manipulator's kinematics in a singularity.
In addition, the mockup capsule followed a U-shaped trajectory, with the desired capsule dipole moment pointing in a direction that forced the robot manipulator into its wrist singularity. The robot manipulator's joint configuration was nearly in the wrist-singular configuration throughout the trajectory. In this demonstration, the Jacobian JFA was ill-conditioned and caused the inverse-kinematics approach using the psuedoinverse (15) to break down, which would result in incorrect magnetic forces being applied to the capsule. By solving the inverse kinematics using the formulation (20)-(22), control over the applied magnetic force was maintained but at the sacrifice of the capsule's heading.
It is to be understood that the above-referenced embodiments are illustrative of the application for the principles of the present invention. Numerous modifications and alternative arrangements can be devised without departing from the spirit and scope of the present invention while the present invention has been shown in the drawings and described above in connection with the exemplary embodiment(s) of the invention. It will be apparent to those of ordinary skill in the art that numerous modifications can be made without departing from the principles and concepts of the invention as set forth in the claims.
This application claims the benefit of U.S. Provisional Application No. 61/852,855, filed Mar. 22, 2013, and U.S. Provisional Application No. 61/837,055, filed Jun. 19, 2013, each of which is incorporated herein by reference.
This invention was made with government support under Grant #0952718 awarded by the National Science Foundation. The Government has certain rights in the invention.
| Number | Date | Country | |
|---|---|---|---|
| 61852855 | Mar 2013 | US | |
| 61837055 | Jun 2013 | US |
