The present invention relates to systems and methods for controlling the movement and positioning of a robotic device. In some particular implementations, the present invention relates to systems and methods for controlling the movement and positioning of a bending robotic device such as, for example, a continuum robot.
In one embodiment, the invention provides a robotic system that includes at least one hollow tube backbone and an equilibrium modulation wire at least partially positioned within the backbone. The robotic system is configured to adjust a position of an end effector by bending the hollow tube and to further adjust the position of the robotic device by adjusting a linear insertion position of the equilibrium modulation wire inside the hollow tube, wherein adjusting the linear insertion position of the equilibrium modulation wire changes a flexural rigidity of the hollow tube resulting in a change in the resulting bending angle of the robotic device.
In other embodiments, the invention provides a robotic system configured to adjust a position of a robotic device by bending a backbone structure of a dexterous arm of the robotic device and to further adjust the position of the robotic device by adjusting a linear insertion position of an equilibrium modulation wire. The equilibrium modulation wire is coupled to the backbone structure of the dexterous arm at least partially along a length of the backbone structure such that adjusting the linear insertion position of the equilibrium modulation wire changes the flexural rigidity along the length of the backbone structure resulting in a change in the resulting bending angle of the robotic device. In some embodiments, the equilibrium modulation wire is coupled to the backbone structure by positioning the equilibrium modulation wire at least partially within an internal channel of the backbone structure. In other embodiments, the equilibrium modulation wire is coupled to the backbone structure by slidably constraining the equilibrium modulation wire along a side of the backbone structure.
In another embodiments, the invention provides a continuum robot system including a plurality of segments, each segment including an end disk, a base disk, a central backbone fixedly attached to the end disk, and a plurality of secondary backbones fixedly attached to the end disk. A bending angle and a bending plane of a first segment is controlled by adjusting a set of linear forces applied to the secondary backbones of the first segment and further modulated by adjusting an internal stored energy of at least one of the secondary backbones that causes a change in the flexural rigidity of the secondary backbone resulting in a change in the resulting bending angle of the first segment achieved by the set of linear forces applied to the secondary backbones of the first segment. In some embodiments, the central backbone is also fixedly attached to the base disk of the segment while, in other embodiments, the central backbone is slidably coupled to the base disk.
Other aspects of the invention will become apparent by consideration of the detailed description and accompanying drawings.
Before any embodiments of the invention are explained in detail, it is to be understood that the invention is not limited in its application to the details of construction and the arrangement of components set forth in the following description or illustrated in the following drawings. The invention is capable of other embodiments and of being practiced or of being carried out in various ways.
Examples of continuum robots and systems and methods for the control and operation of a continuum robot are described in detail in U.S. Publication No. 2014/0316434 to Simaan et al. and U.S. Publication No. 2014/0330432 to Simaan et al., the entire contents of which are incorporated herein by reference.
Although, in the perspective of the inset illustrated in
In the example of
In the following sections we will use q∈Rn to designate the active joint values responsible for macro motion control. The vector q−∈Rp is used to designate the micro-motion joint values responsible for equilibrium modulation. The mechanical energy of the continuum segment is given predominantly by its elastic energy because gravitational energy effects are negligible for small robots of the size in this example. Assuming n backbones in a continuum segment and using index i=0 to refer to its central backbone, the elastic potential energy is given by:
u=½Σi=0n∫0L
where Ki, Ei, Ii, and Li designate curvature, Young's modulus, bending moment of inertia, and the length of the i'th backbone, respectively. The variable s stand for the arc-length parameter s∈[0, Li]. We also note that the length of the i'th secondary backbones is given as a function of the joint variables Li=L0+q, i∈[1, n].
The shape of the continuum segment is given by ko(s) since the shapes of the secondary backbones as given by ki(s), i∈[1, n] are geometrically constrained to obey constant distance from the primary backbone. This shape is given as a solution to a variational problem minimizing the total elastic energy of the segment. In general, this shape is very close to circular (i.e., k0 is approximately constant). An exact shape can be obtained using an elliptic-integral constrained optimization formulation.
The macro-motion direct kinematics (DK) of the robot illustrates in the examples of
x=f(ψ(q)) (2)
The instantaneous direct kinematics (IDK) relates the macro motion joint speeds with end effector twist according to the equation:
{dot over (x)}=g({dot over (ψ)}(q,{dot over (q)})) (3)
In both cases the transition from joint space to task space variables is achieved by using the configuration space coordinates ψ and the configuration space speeds {dot over (ψ)}. Specifically, two Jacobian matrices are calculated in closed-form such that:
{dot over (x)}=J
xψ{dot over (ψ)} (4)
{dot over (q)}=J
qψ{dot over (ψ)} (5)
In macro actuation mode, the macro-motion actuator control q while the micro-motion actuators maintain {tilde over (q)} fixed. For a given value of {tilde over (q)}, the minimum energy bending shape of the continuum segment is given by minimizing the following equation:
u=½[Σi=0n∫0L
where ki(s) designates the local curvature of the i'th backbone of the bent segment, Ei, Ii, and {tilde over (E)}i, Ĩi are the Young's moduli and bending moments of inertia of the outer tube (macro-motion backbone) and the equivalent Young's modulus of the ñ micro-motion backbones in the portions where they overlap (si=0 . . . {tilde over (q)}i).
To solve the problem of finding the equilibrium shape we will initialize our solution using the circular shape determined by the nominal solution of the inverse kinematics mapping. In addition, the solution must satisfy geometric and static equilibrium constraints leading to a constrained nonlinear minimization formulation:
minimize u=½[Σi=0n∫0L
Subject to: g(k)=0, where k=[k1, . . . kn]T. where the constraint g(k) is the boundary constraints specific to the robot architecture and materials. A constrained nonlinear minimization algorithm can be used to find the approximate solution for k(s). The Augmented Lagrangian method or Active Set sequential programming method might also be used for the numerical solution by assuming a modal representation of k(s) such that:
The outcome optimization algorithm will result in the modal coefficients {tilde over (c)}i and ci which define the curvature ki(si). Given the curvature ki(si), we will use differential geometry to define the Jacobians which will be used for resolved rates control according to equations (4) and (5) above.
In micro-motion control mode, the macro-motion joints q are locked while the micro-motion joints q− are used to change the elastic energy and thereby change the equilibrium pose of the robot. Equation (6) shows that u=u (q, {tilde over (q)}). The complete differential u becomes:
du=(∇qu)Tdq+(∇{tilde over (q)}u)Td{tilde over (q)}=(∇{tilde over (q)}u)Td{tilde over (q)} (9)
Equation (9) shows that, in micro motion mode, the change in elastic energy depends on the gradient of the energy with respect to the micro-motion degrees of freedom.
To capture the change in the shape of the backbones when {tilde over (q)} is changed, we use the modal representation in equation (8) and we express the unknown coefficients ci and {tilde over (c)}i as a function of q using the modal representation ci=Λiϕ({tilde over (q)}) where Λ∈x×y and ϕ({tilde over (q)})∈y. This representation results in expressing the curvature as:
The solution to the constrained energy minimization problem will give the modal coefficients A and ki(si). Given the curvature ki(si), the Jacobians that define the micro-motion kinematics can be defined using differential geometry.
To summarize, the degree of bending of the segment caused by linear movement is related to the rigidity of the central backbone, the secondary backbones, and the equilibrium modulation wires positioned inside the secondary backbones. As illustrated in
In the example of
Accordingly, in some implementations, macro-motion control is used as a primary mechanism for adjusting the pose of the continuum robot and/or an individual continuum robot segment by applying a linear force to push and/or pull the secondary backbones of a given segment. However, macro-motion control is limited in terms of resolution and precision. Therefore, micro-motion control is used to adjust the rigidity of one or more of the secondary backbones by advancing/retracting the equilibrium modulation wires causing the segment of the continuum robot to move from a first equilibrium pose into a second equilibrium pose. By adjusting the rigidity and the equilibrium of the continuum robot segment, the micro-motion control provides for fine tuning adjustments and control of the continuum robot and an increased level of position control and resolution for the continuum robot. Furthermore, just as the forces applied to the secondary backbones during macro-scale motion control are adjusted in coordination to achieve a target bending angle and bending plane for the segment, the linear positions of the equilibrium modulation wires in each of the secondary backbones are also controlled and adjusted in coordination to achieve a target bending angle adjustment and bending plane adjustment under micro-motion control.
The macro-scale motion actuator 405 includes a plurality of linear slide actuators that each apply a linear force to a respective one of the secondary backbones based on an output instruction/signal from the electronic processor 401. The micro-scale manipulation actuator 407 also includes a series of linear slide actuators that are each coupled to a respective one of the equilibrium modulation wires based on an output instruction/signal from the electronic processor 401. The electronic processor 401 is configured to generate the output instruction/signals to the macro-scale motion actuator 405 and to the micro-scale manipulation actuator 407 based in part on a user adjustment of the user input control 409 (e.g., a joystick or other input device).
As discussed above, the continuum robot can be equipped with a working tool at its distal end. Accordingly, the end-effector 411 can include, for example, a gripper, a laser device, or an imaging device such as an OCT probe. In some implementations, the continuum robot may be equipped with a camera system positioned at its distal end in addition to or instead of an end-effector 411/working tool.
In this example, the micro-scale manipulation actuator 407 includes three automated linear slides. The linear slides in this example are VELMEX A1506B-S1.5 linear slides equipped with a 4.5″ 20 turns/inch travel lead screw and are driven by a Maxon Motor RE16 4.5W equipped with planetary gearhead GP16A 19:1 and an MR encoder with 256 counts per turn.
After the position adjustments for the secondary backbones and the linear position adjustments for the equilibrium modulation wires are determined, the electronic processor 401 actuates the macro-scale motion control of the continuum robot by causing the macro-scale motion actuator 405 to apply the determined set of forces to the secondary backbones (step 607). Once the macro-scale adjustment is complete, the electronic processor 401 actuates the micro-scale motion control by causing the micro-scale manipulation actuator 407 to adjust the linear positions of the equilibrium modulation wires (step 609).
In the example of
In some implementations, the system is configured to move adjust the pose of the continuum robot using macro-scale motion control until the system determines that the pose of the continuum robot is close enough to the target pose that the target pose can be achieved by micro-scale control. In some such implementations, this check is performed between step 607 and step 609 in the example of
In still other implementations, the control system may be configured to perform the micro-scale motion control based on feedback after the macro-scale motion adjustment is performed. For example, instead of determining target adjustments to the linear position of the equilibrium modulation wires before applied the macro-scale adjustment, the control system may be configured to first apply the macro-scale motion adjustment and then determine an error between the macro-scale adjusted pose and the actual target pose (using, for example, feedback from a camera system 413 or other position sensors). The control system would be configured to then further adjust the pose of the continuum robot using micro-scale control based on the determined position error until the error is reduced to below a defined acceptable threshold. In some implementations, the system is configured to use data collected by one or more sensors coupled to the distal end of the continuum robot for the feedback mechanism including, for example, image data from an OCT probe or other vision/camera sensor.
Finally, although the examples discussed above include equilibrium modulation wires in each of the secondary backbones, other implementations may include more or fewer equilibrium modulation wires. For example, a system that includes 6 secondary backbones for each segment may only include equilibrium modulation wires in three of the six secondary backbones. Conversely, some implementations may be configured to include an equilibrium modulation wire in the central backbone to adjust the rigidity of the continuum robot in addition to or instead of the equilibrium modulation wires in the secondary backbones.
Furthermore, although the examples described above achieve micro-scale motion control through equilibrium modulation by adjusting a linear position of an equilibrium modulation wire within an internal channel of a backbone structure, other mechanisms can be implemented to attain equilibrium modulation by adjusting the stored energy of the system in other implementations. For example, in some implementations, the equilibrium modulation wire can be slidably coupled to the side of a backbone and moved linearly to adjust the flexure rigidity. In other implementations, the system may be configured to apply thermal energy (e.g., heat) to the backbone structure to adjust its rigidity and the resulting equilibrium position. In still other implementations, the system may be configured to adjust a pressure of a fluid or gaseous medium within an internal chamber along the length of the backbone structure to adjust its rigidity and the resulting equilibrium position.
As discussed above, a continuum robot with macro/micro scale motion control can be integrated into a medical imaging system such as, for example, an OCT system. For example, a B-mode OCT probe may be coupled to the distal end of the continuum robot's “snake-like” body. For example, an OCT probe may be constructed with an outer diameter of 0.66 mm and configured to fit through the central backbone of the continuum robot. The OCT system may also include, for example, a spectral-domain optical coherence tomography system (e.g., a 870 nm system by Bioptigen, Inc., Morrisville, N.C.) coupled to the OCT probe and used to generate a synchronization signal, for image acquisition, and for generation of the B-mode images.
In some B-mode OCT systems, the probe is configured to perform lateral scanning by pivoting a mirror. However, lateral scanning can be performed without a pivoting mirror by instead using an OCT probe coupled to the distal end of a continuum robot with macro/micro scale motion control as described above. For example, lateral scanning for OCT can be achieved with sufficient speed and positioning control by bending a segment (e.g., the distal segment) of the continuum robot using macro-scale control and then adjusting the bend angle while capturing data with the OCT probe by adjusting the linear position of the equilibrium modulation wire(s). As discussed above, the change in equilibrium will cause small, precision-controlled movements of the distal end of the continuum robot that are suitable for OCT scanning.
In some implementations, the system may be configured to provide back-and-forth reciprocating movements of the distal end of the continuum robot (e.g., for OCT image data capture or other procedures using other end effectors/working tools) by alternatingly inserting and withdrawing one or more of the equilibrium modulation wires between two defined linear positions.
The examples described above present the concept of continuum robot equilibrium modulation (CREM) and provide a visual measurement solution to observe macro-motion. The examples below outline a modeling approach that can explain the experimental observations and that can be used for control and identification purposes. These examples present a simplified kinematic modeling framework that captures the micro-motion achieved by the equilibrium modulation of continuum robots and develop a calibration approach to capture the model parameters. The concept of “moment coupling effect” is presented as a simplified approach to describe the equilibrium modulation behavior and, therefore, both direct kinematics and instantaneous kinematics are formulated for control purposes. To account for errors potentially caused by the simplistic modeling assumptions, a modeling uncertainty term is introduced, and the identification Jacobian along with a calibration framework to capture the parameterization is developed. Using the multi-backbone continuum robot design as a validation platform, we validate the kinematic model and model calibration experimentally while augmenting these results with additional simulation validations.
This section presents the bending shape equilibrium modeling in the case where the Equilibrium Modulation Backbone (EMB) insertion is at a given depth qs. When the EMBs are not inserted and for proper design parameters (e.g. small spacing between the spacer disks) the continuum segment bends in a constant curvature. We use this assumption to create a simplistic equilibrium model which lends itself to fast realtime computation. Since we have to account for modeling uncertainties due to friction and material parameter uncertainties, we later lump the error of the simplified model in an uncertainty term X, that will be used to produce an updated CREM model.
In
We first consider the resultant moment m1 that the backbones apply on any imaginary cross section of the continuum segment when no EMB is inserted (
where Ep, Ei and Ip, Ii denote the Young's moduli and cross-sectional bending moments of inertia of the central backbone and the ith secondary backbone, respectively. Also L and Li denote the lengths of the central backbone and the ith secondary backbone.
We also consider the moment m1′ along the empty subsegment in the case of EMB being inserted (
where Lε
The radial distance between the secondary backbones and the primary backbone is denoted r. When r is projected onto the plane in which a segment bends, we obtain the projected radial distance Δi:
Δi=r cos(σi), σi=δ+(i−1)β (13)
where σi designates the angular coordinate of the ith backbone relative to the bending plane. The angular coordinate of the first backbone relative to the bending plane is δ and the angular separation between secondary backbones is
where n is the number of secondary backbones.
The length of the ith backbone, L is derived using the fixed radial offset between the backbones:
L
i
=L+Δ
i(θ−θi) (14)
Using similar rationale, we calculate the empty length portion Lε
L
s
=q
s+Δi(θs−θ0) (15)
L
s
=(L−qs)+Δi(θ′−θs)=Li−Ls
In both
We next use a simplifying assumption that the effect of EMB wire insertion on changes in the bending curvatures of the un-inserted subsegment backbones is negligible, hence:
m
1
=m
1′ (17)
Next, we consider m2 and ms, the moments that the secondary backbones and the EMB apply on the separation plane as shown in
where Es and Is denote the Young's modulus and cross-sectional bending moment of inertia of the EMB.
Substituting equations (11, 12) into (17), results in one equation having two unknowns θ′ and θs as illustrated in
m
1
′+m
2
+m
s=0(20)
To solve equations (20) and (17) for the unknowns θ′ and θs we explicitly express the backbone moments using the beam equation m=EIk where k designates the radius of curvature and EI designates the bending cross sectional stiffness of a beam. In doing so, we note that the curvature of a beam bent in a circular shape satisfies
where θ is the deflection angle and L is the beam length. Since the backbone lengths are a function of the unknowns, we will rewrite the moment equation for a beam as
and, by defining the beam angular deflection stiffness
we obtain a simple equation for the moment m=kθθ.
Using the above definition for beam angular deflection stiffness, we rewrite the moment equations for each beam as:
Substituting equation (22)-(24) in equation (20) results in:
Substituting equation (21) and (22) in equation (17) results in:
As a final step in the solution, we substitute the result in equation (26) in equation (25), thereby obtaining θs and subsequently θ′.
Equations (25) and (26) present the solution to the simplistic modeling approach that is fundamentally based on Eq. (17) and Eq. (20). In addition to the simplified assumption, the current model also neglects modeling uncertainties due to frictional effects and material property uncertainties. These uncertainties include friction and strain along the actuation lines, non-uniformly distributed load on backbones that causes shape deviations from constant-curvature bending, deviations in the cross section of the backbones during bending, and uncertainties in the properties of the NiTi backbones.
To account for the modeling uncertainties caused by friction, material uncertainty, and our simplistic model, we introduce an uncertainty term λ to equation (20):
m
1
′+m
2
+m
s=λ(qs,θ,kλ) (27)
The uncertainty term λ captures effects of EMB insertion offset, bending angle uncertainty and a fixed offset:
λ(qs,θ,kλ)=kλ
The solution in equation (25) is also updated to:
Having obtained the solutions to the equilibrium tip bending angle θ′ and the bending angle at the separation plane θs, we introduce an equilibrium configuration space variable vector Φ to combine them. With the purpose of preparing for kinematic derivations in later sections when we break a single continuum segment down to two subsegments, the vector Φ is defined as:
where θε represents the bending angle of the empty subsegment.
We define the configuration space variable w as the nominal bending angle θ and the bending plane angle δ:
ψ≙[θ,δ]T (31)
Finally, the solution to equilibrium modeling problem is presented as a mapping ℑeqm which is used in deriving the Jacobian matrices in the following sections:
ϕ=ℑeqm(ψ,qs,kλ), ϕ∈2,ψ∈2,kλ∈3 (32)
Equation (32) provides the end disk equilibrium angle for a combination of any given EMB insertion length qs, nominal bending angle θ, and bending plane angle δ.
Kinematic modeling of CREM includes the mapping of configuration space to task space and its differential kinematics. The differential kinematics include the instantaneous kinematics and the error propagation.
The instantaneous kinematics is derived for control purpose, and it includes two motion Jacobian matrices that both relate actuation speeds to the robot tip velocity. The macro motion Jacobian JM is associated with the joint velocities of push/pull on backbones (direct actuation) while the micro motion Jacobian Jμ is related to the EMB insertion velocity (indirect actuation).
The kinematic error propagation investigates how errors in parameters contribute to errors in kinematic measurements of task space (e.g. measured positions). In this work, we focus on the vector kλ that parameterizes the modeling uncertainty. An identification Jacobian Jk is derived and used in the discussion below to estimate kλ with experimental measurements.
With the mapping ℑeqm in equation (32) derived as the result of static equilibrium, the kinematic mapping can be formulated by considering a single continuum segment as two concatenated subsegments—the inserted and the empty, divided at the insertion depth qs.
δs=δε=δ (33)
The kinematic nomenclature used in
Because the direct kinematics of a single segment have a length Lx and an end disk angle θx, the end disk pose (i.e., position and orientation) relative to the base are given by:
where δx designates the angle of the bending plane (analogous to δ in
To obtain the pose of the end disk of the inserted segment is given by bpc/b and bRc we substitute Lx=qs, θx=θs, δx=δ in equations (34), (35). Similarly, the pose of the end disk of the empty segment relative to its base is obtained by substituting Lx=L−qs, θx=θε, δx=δ in equations (34), (35) to result in cpg/c and cRg.
The pose of the free subsegment end disk relative to the segment base is given by:
b
p
g/b=bpc/b=bRccpg/c≙ℑg(ϕ,δ,qs) (36)
b
R
g=bRccRb=Rg(ϕ,δ) (37)
Casting the above two equations in a homogeneous transform format yields:
With Φ expressed using mapping ℑeqm(ψ, qs, kλ), the forward kinematics is determined, which can be also written as:
b
T
g≙ℑT(ψ,qs,kλ) (39)
The total differential of a homogenous transform T∈SE(3), may be represented as:
where dx and dμ represent translational and rotational differentials in the base frame. The vector μ∈3×1 represents a chosen orientation parametrization (e.g., Euler angles).
The total differential of bTg is obtained by considering differentials on all variables, i.e., dΦ, dδ, and dqs:
Using the nomenclature of a Jacobian Jab such that δa=Jabδb, we define the following Jacobian matrices:
The Jacobian matrices Jξϕ, Jξδ, and Jξq
The third Jacobian, Jξq
where
are derived from equation (34). It is important to note that Jξq
Having derived the above three Jacobian matrices, Jξϕ, Jξδ, and Jξq
where the gradient terms are derived, as discussed below, as:
Matrices Ax′ and Bx′ are partial derivative matrices with respect to variable “x”, and A, S0, B, C0, and S1 are defined as:
Using equations (47)-(53), dΦ is fully expressed as equation (45). Substituting dΦ into the original differentiation in equation (42), we obtain the full differential kinematics that relates differentials on (ψ, qs, kλ) to the pose total differential d:
Rewriting equation (55) using the Jacobian definitions:
dξ=J
ξϕ
J
ϕθ
dθ+J
ξϕ
J
ϕδ
dδ+J
ξδ
dδ+J
ξϕ
J
ϕq
dq
s
+J
ξq
dq
s
+JξϕJ
ϕk
dk
λ (55)
Collecting like terms of dψ, dqs, and dkλ, we obtain a differentiation that decouples differentials of the macro motion, the micro motion, and the parameters:
The above result completes the mapping from configuration to task space. It clearly delineates the effects of EDM insertion and direct actuation to achieving macro and micro motion. For control purposes, a complete mapping from joint to task space is needed. We therefore consider next the mapping from direct (macro) actuation joint space q to task space ξ. Since three secondary backbones are used in our experimental setup as illustrated in
q
i
≙L
i
−L (57)
When obtaining this mapping, we consider the nominal segment kinematics for multi-backbone continuum robots.
The Jacobian that relates the differential dq to the differential dψ is:
where r denotes the constant radial distance between the central and surrounding backbones, and β=2π/3 denotes the backbone separation angle. Using equation (58), we substitute div into equation (56), arriving at the final differential kinematics:
dξ=J
M
dq+J
μ
dq
s
+J
k
dk
λ (59)
Equation (59) fully decouples the end-effector pose differential to contributions of the direct (macro) actuation dq, the indirect (micro) actuation dqs, and the modeling uncertainty dkλ. The three Jacobian matrices are obtained from equation (59): JM defined as the macro motion Jacobian, Jμ defined as the micro motion Jacobian, and Jk defined as the Identification Jacobian.
J
M=[JξϕJϕθJξϕJϕδ+Jξδ](Jqψ)†, JM∈6×2 (60)
J
μ
=J
ξϕ
J
ϕq
+J
ξq
, J
μ∈6×1 (61)
J
k
=J
ξϕ
J
ϕk
, J
k∈6×n
where (*)† is the Moore-Penrose pseudo inverse.
The result in equation (59) builds on knowing the Jacobians Jξϕ∈6×2 and Jξδ∈6×1 as mentioned in equations (42) and (43). We now provide a derivation to these two Jacobians. Considering a single-segment CREM as two concatenated subsegments (inserted and empty), we apply the Jacobian formulation for the two-segment multi-backbone continuum robot (MBCR) while assuming that both subsegments share the bending plane angle δ. For ease of adapting formulations, we introduce a vector notation:
We next use the notation of i-1ξi/i-1 to denote the pose of the ith subsegment relative to the (i−1)th subsegment where i∈{s,ε}. Using v and ω to denote linear and angular velocities, we define the corresponding four Jacobian matrices corresponding with the contributions of dθi, dδi, where i∈{s,ε} to the end-effector twist:
Details of the derivations of the Jacobians are provided below.
The serial composition of two subsegments using twist transformation results in the end effector twist:
These definitions of Jξϕ and Jξδ
To calibrate the model uncertainty parameters kλ, we extract from equation (59) the following relation:
δξ(δkλ)=Jkδkλ (68)
Using this error propagation model, we construct an estimation method to estimate kλ. Let ξj↔[
c
j≙[(
where αe
where the operator (*)V designates the vector form of a skew-symmetric matrix.
An aggregated error vector {tilde over (c)}λ is defined to include errors of all N robot configurations:
{tilde over (c)}
λ=[(c1)T, . . . ,(cN)T]T (73)
The optimization objection function MA is then defined as:
where W is a weight matrix encoding confidence in the measurements and the measurement unit scaling factors.
The first-order Taylor series approximation of Ma is given:
M
λ(kκ+kK)≈Mλ(kλ)+JM
where the aggregated Jacobian JM
Equation (76) shows that minimizing Mλ, entails following the gradient descent direction along d{tilde over (c)}λ/dkλ. The parameter kλ is then obtained using an iterative nonlinear least squares estimation shown in algorithm of
In the examples below, we present simulations to demonstrate the direct kinematics and differential kinematics. We also verify the differential kinematics through finite-difference simulations. We also carry out simulations to verify the differential kinematics model. In all simulations, we assumed the robot points vertically down at its home (straight) configuration.
Using the model described above, we present the simulated position analysis of the micro motion created by the EMB insertion. In both simulations and the experimental model validations, we sue the parameters as in Table 2. They include the Young's modulus of the superelastic NiTi material used for backbone tubes and EMBs (Ep, Ei, Es), the diameters of the backbones (dp, di, ds), and the cross-sectional moment of inertia (Ip, Ii, Is).
To verify the derivations of instantaneous kinematics and error propagation, we compute Jacobians as described above. Since the simulation case represents the robot motion within a bending plane, the columns of the Jacobians represent 2×1 vectors of induced velocities for unit change in the variables associated with each Jacobian. The following simulations verify the macro motion Jacobian JM, the micro motion Jacobian Jμ, and the identification Jacobian Jk by plotting the Jacobian columns. The verification is carried out visually by verifying that the Jacobian columns induce tip velocity tangent to the trajectory generated by direct kinematics. In addition, each Jacobian has been also verified numerically via finite difference computations.
To verify JM, the EMB insertion depth qs was fixed and direct actuation of backbones was assumed. Sample tip positions along the trajectory were obtained via direct kinematics and the corresponding Jacobian JM was computed.
To verify Jμ, the secondary backbones were assumed locked and the EMB insertion depth qs was varied. The Jacobian Jμ was computed and plotted for each EMB depth. Two different scenarios of modeling uncertainty were considered: λ=0 and λ=0.2+0.06qs. The results in
The following examples evaluate the ability of our simplified kinematic model to capture the micro-motion behavior, validate the calibration framework, and assess the accuracy of the updated kinematic model in reflecting experimental data. A single-segment continuum robot with EMB insertion actuation was used as the experimental platform as shown in
Using the calibration method described above, we calibrated the modeling uncertainty parameter vector kλ. The parameter vector kλ in equation (28) consists of three elements, a bias term kλ
that relates to the EMB insertion depth qs. As a preliminary study, in this paper, we focus on investigating and calibrating kλ
Once the characterization of kλ
is achieved, one can exhaust the choices of q to investigate the effect of kλ
The algorithm of
In each iteration, the modeled positions were computed using the current estimates of the parameters. The aggregated error vector was then calculated between the modeled and experimental positions. For each iteration, both of the current estimates of the parameters and the position root-mean squared errors (RMSE) of all insertion samples (382 in total) were reported. A relative convergence threshold of 0.1% was used to determine the convergence.
For the particular experimental data collection, the parameter estimation (model calibration) went through 46 iterations before converging, where a step size of 11=0.1 was selected.
By dividing the tip trajectory into two segments, we observe that the current simplistic modeling approach produced bigger errors after the turning point: the RMSEs were reported as 4.87 μm and 6.63 μm for the two segments before and after turning point that had the lengths of 48.11 μm and 38.82 μm, respectively. If one wishes to further improve the model accuracy, a model that only predicts the trajectory before the turning point may be considered. We therefore considered another calibration where only the partial micro motion trajectory before the turning point was used. With the same iteration step size and convergence criterion, the estimation went through 59 iterations to converge, and the updated results are reported in Table 4 and plotted in
This work focused on creating a simplistic, yet fast model for equilibrium modulation control implementation. The kinematic model traded accurate mechanics modeling (which typically leads to solving nonlinear boundary value problems) with simplicity and speed of computation. Our experimental data showed an unexpected motion behavior manifested by a turning point along the micro-motion trajectory. The model presented in this paper does not offer a physical explanation to this behavior, but can capture this behavior for a given robot. The model calibration results indicate that there is still a potential to improve the model performance by further investigating alternative modeling assumptions and different descriptions of modeling uncertainties. One of the limitations of our approach can be inferred from the simulation shown in
This work presented the first modeling attempt for a new class of continuum robots capable of multi-scale motion. These robots achieve macro-scale and micro-scale motions through direct and indirect actuation (equilibrium modulation). Instead of focusing on a high-fidelity mechanics-based model, which typically leads to non-linear boundary value problems not easy to adopt for real-time control or parameter identification. Instead the paper presented a simplified mechanics-based model utilizing moment coupling effects between sub-segments of the continuum robot. This approach generates a differential kinematics model that covers both macro and micro-motion. As a result of unavoidable parameter uncertainty, we presented a model-calibration approach that can compensate for parameter inaccuracy, friction effects and modeling inaccuracies due to the simplistic modeling assumptions. The modeling approach along with the calibration framework was validated experimentally on a multi-backbone continuum robot. The calibrated model reported a positional root-mean-squared error as 5.83 μm if one wishes to use the model for the entire motion profile with the turning point. If one chooses to exclude motions past the turning point, the calibrated model fit the experimental data with an accuracy of 4.76 μm. Future work will focus on investigations on a more sophisticated models capable of incorporating geometric constraints as well as minimizing mechanical energy for improved model accuracy. In addition, effects of direction reversal of EMB insertion can manifest in hysteresis.
As mentioned above, the gradient terms defined in equations (47)-(51) are derived as follows. Rewriting equations (29) and (26) in a matrix form yields:
where A and B are defined as functions of {ψ, qs, θs} and {ψ, qs, kλ, θs} respectively. Using the definition of ϕ, yields:
By introducing two constant matrices in the above equation, S0 and C0, we have obtained the equation to differentiate:
A(S0ϕ−C0)=B, A∈2×2, B∈2, C0∈2 (80)
The full differentiation may be expressed as:
(dA)(S0ϕ−C0)+(AS0)dϕ=dB (81)
Using Xa′ to denote the partial derivative of matrix X w.r.t the scalar variable a, i.e.
then dA and dB may be written as:
dA=A
θ
′dθ+A′
δ
dδ+A
q
′dq
s
+A
θ
′dθ
s (82)
dB=B
θ
′dθ+B
δ
′dδ+B
q
′dq
s
+B
θ
′dθ
s+Σin
Let us define Cϕ and Γa to provide ease in the derivations:
C
ϕ
S
0
ϕ−C
0, Γa=Ba′−Aa′Cϕ (84)
where the letter a∈{θ, δ, qs, θs}.
By substituting equation (82) and equation (83) into equation 81, and by using the definitions of Cϕ and Γa, we have:
This equation shows the full differentiation of equation (45) and all the Jacobians can be obtained directly by their definitions, i.e., the expressions of
may be written as described above in equations (47), (48), (49), and (50), which s1=[1,0] is just a selection matrix.
Furthermore, as mentioned above, equations (64), (65), and (66) refer to the Jacobian matrix partitions for the two-segment case where the first segment is the inserted subsegment (indicated by superscript s) and the second segment is the empty subsegment (indicated by subscript c). The Jacobian partitions represent the effects of differentials on θi and δi that contribute to the end-effector's translational and rotational differential, labeled by subscripts ‘v’ and ‘ω’, indicating ‘velocity’ and ‘angular velocity’, respectively. The expressions of {Jvθ
Where c(⋅) and s(⋅) denote the cosine and sine functions, and Di represents the length of the subsegment. For the inserted subsegment, Ds=qs; and for the empty subsegment, Dr=L−qs. In addition, the following shorthanded notations are used:
Thus, the invention provides, among other things, a system for multiple-scale motion control of a bending robotic device using equilibrium modulation. Various features and advantages of the invention are set forth in the following claims.
This application is a continuation of U.S. patent application Ser. No. 16/647,278, filed Mar. 13, 2020, entitled “CONTINUUM ROBOTS WITH MULTI-SCALE MOTION THROUGH EQUILIBRIUM MODULATION,” which is the U.S. national stage entry, under 35 U.S.C. § 371, of International Patent Application No. PCT/US2018/050948, filed Sep. 13, 2018, which claims the benefit of U.S. Provisional Patent Application No. 62/557,969, filed Sep. 13, 2017, entitled “CONTINUUM ROBOTS WITH MULTI-SCALE MOTION THROUGH EQUILIBRIUM MODULATION,” the entire contents of each of which are hereby incorporated by reference.
This invention was made with government support under grant CMMI-1537659 from the National Science Foundation (NSF). The government has certain rights in the invention.
Number | Date | Country | |
---|---|---|---|
62557969 | Sep 2017 | US |
Number | Date | Country | |
---|---|---|---|
Parent | 16647278 | Mar 2020 | US |
Child | 17187534 | US |