Patent Application
20220080591
The present disclosure is directed to systems and methods for performing a robotic procedure, and more particularly to systems and methods for controlling more particularly to systems and methods for controlling a robotic manipulator or a tool associated with a robotic manipulator.
Robotic manipulator assemblies can be operated to control motion of tools in a workspace. For example, such robotic manipulators can be used to perform non-medical and medical procedures. As a specific example, teleoperated surgical manipulators can be used to perform minimally invasive medical procedures.
It is desirable in medical techniques are intended to reduce the amount of tissue that is damaged during medical procedures, thereby reducing patient recovery time, discomfort, and harmful side effects. For example, minimally invasive techniques may be performed through natural orifices in a patient anatomy or through one or more incisions. Through these natural orifices or incisions, clinicians may insert medical tools to reach a target tissue location. Minimally invasive medical tools include tools such as therapeutic tools, diagnostic tools, and surgical tools. Minimally invasive medical tools may also include imaging tools such as endoscopic tools that provide a user visualization within the patient anatomy.
Robotic manipulators may be teleoperated or otherwise computer-assisted. For example, a tool may be held by a robotic manipulator assembly for performing a procedure. However, flexibility in such a robotic manipulator assembly and the tool may cause under-damped vibrations with undesirably long settling times. Flexibility (compliance) may be used to measure physical compliance, mechanical compliance, structural compliance, and the ability to deflect under load. In examples where the robotic manipulator assembly (including its base, joints, and links) has a relatively large flexibility and/or has relatively large link masses or inertias (e.g., greater than corresponding thresholds of flexibility/link masses/inertias), motions commanded of the robotic manipulator assembly or external disturbances may cause such vibrations. As such, a combination of physical parameters including compliance (e.g., physical, mechanical, and structural compliance), damping (e.g., physical damping including viscous damping, where viscous damping elements (e.g., lubricant friction or structural damping) resist motion with forces proportional to the velocity of motion), and mass/inertia results in a low mechanical resonance with less than desired damping. During a procedure, commanded motions or external disturbances may excite these low mechanical resonances causing undesirable vibrations. While performing a procedure, such vibrations experienced at the tool tip or another point of control on the robot manipulator may result in inferior system performance. For example, such vibrations may make it difficult for the computer-assisted system to achieve or follow commanded trajectories for the tool.
Such vibrations may negatively affect control in all types of robotic systems, including medical robotic systems. In a medical robotic example, such vibrations may make it difficult for a medical robotic system to perform the commanded manipulations of tissue, movement of imaging systems such as endoscopes and ultrasonic probes, insertion of needles, application of sutures, etc. For a further example, in some implementations, the tool is moved around a remote center of motion (also referred to as “remote center”) during part or all of a procedure. In some instances, the vibrations may cause the remote center of motion to move during surgery imparting undesirable forces on the body wall at the port of entry. The vibrations may cause the actual pose or motion of the tool (or a manipulator holding the tool) to deviate from the commanded posed or motion to such an extent that the tool (or manipulator) acts as if the remote center has moved beyond a predetermined tolerance from its defined position. That is, the range of location of the virtual fulcrum associated with the motion of the manipulator and/or the tool is caused by the vibrations to exceed a tolerance amount from a defined remote center of motion.
Thus, systems and methods are desired to provide better control of robotic systems, such as better control of a manipulator of a robotic system by mitigating undesirable vibrations, better control of the tip of a tool held by the robotic system (where the tool is a camera or other imaging device, better control of the tip can provide improved image stabilization), and the motion of the manipulator or a tool supported by the manipulator about the remote center of motion of the tool.
The embodiments of the invention are summarized by the claims that follow below.
In one illustrative embodiment, a system includes a robotic manipulator including a serial chain comprising a first joint, a second joint, and a first link. The system further includes a processing unit including one or more processors. The processing unit is configured to receive first link data from a first sensor system located at the first link, generate a first joint state estimate of the first joint based on the first link data, and generate a second joint state estimate of the second joint. The processing unit is further configured to apply a first weight to the first joint state estimate to generate a first weighted joint state estimate, apply a second weight to the second joint state estimate to generate a second weighted joint state estimate, and control the first and second joints based on the first weighted joint state estimate and second weighted joint state estimate.
In another illustrative embodiment, a method comprises receiving first link data from a first sensor system located at a first link of a robotic manipulator including a serial chain comprising a first joint, a second joint, and the first link. A first joint state estimate of the first joint is generated based on the first link data. A second joint state estimate of the second joint is generated. A first weight is applied to the first joint state estimate to generate a first weighted joint state estimate. A second weight is applied to the second joint state estimate to generate a second weighted joint state estimate. The first and second joints are controlled based on the first weighted joint state estimate and second weighted joint state estimate.
In another illustrative embodiment, a non-transitory machine-readable medium comprising a plurality of machine-readable instructions which, when executed by one or more processors, are adapted to cause the one or more processors to perform a method. The method comprises receiving first link data from a first sensor system located at a first link of a robotic manipulator including a serial chain comprising a first joint, a second joint, and the first link. A first joint state estimate of the first joint is generated based on the first link data. A second joint state estimate of the second joint is generated. A first weight is applied to the first joint state estimate to generate a first weighted joint state estimate. A second weight is applied to the second joint state estimate to generate a second weighted joint state estimate. The first and second joints are controlled based on the first weighted joint state estimate and second weighted joint state estimate.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory in nature and are intended to provide an understanding of the present disclosure without limiting the scope of the present disclosure. In that regard, additional aspects, features, and advantages of the present disclosure will be apparent to one skilled in the art from the following detailed description.
Aspects of the present disclosure are best understood from the following detailed description when read with the accompanying figures. It is emphasized that, in accordance with the standard practice in the industry, various features are not drawn to scale. In fact, the dimensions of the various features may be arbitrarily increased or reduced for clarity of discussion. In addition, the present disclosure may repeat reference numerals and/or letters in the various examples. This repetition is for the purpose of simplicity and clarity and does not in itself dictate a relationship between the various embodiments and/or configurations discussed.
For the purposes of promoting an understanding of the principles of the present disclosure, reference will now be made to the embodiments illustrated in the drawings, and specific language will be used to describe the same. It will nevertheless be understood that no limitation of the scope of the disclosure is intended. In the following detailed description of the aspects of the invention, numerous specific details are set forth in order to provide a thorough understanding of the disclosed embodiments. However, it will be obvious to one skilled in the art that the embodiments of this disclosure may be practiced without these specific details. In other instances, well-known methods, procedures, components, and circuits have not been described in detail so as not to unnecessarily obscure aspects of the embodiments of the invention.
Any alterations and further modifications to the described devices, tools, methods, and any further application of the principles of the present disclosure are fully contemplated as would normally occur to one skilled in the art to which the disclosure relates. In particular, it is fully contemplated that the features, components, and/or steps described with respect to one embodiment may be combined with the features, components, and/or steps described with respect to other embodiments of the present disclosure. In addition, dimensions provided herein are for specific examples and it is contemplated that different sizes, dimensions, and/or ratios may be utilized to implement the concepts of the present disclosure. To avoid needless descriptive repetition, one or more components or actions described in accordance with one illustrative embodiment can be used or omitted as applicable from other illustrative embodiments. For the sake of brevity, the numerous iterations of these combinations will not be described separately. For simplicity, in some instances, the same reference numbers are used throughout the drawings to refer to the same or like parts.
Although some of the examples described herein often refer to surgical procedures or tools, or medical procedures or tools, the techniques disclosed also apply to non-medical procedures and non-medical tools. For example, the tools, systems, and methods described herein may be used for non-medical purposes including industrial uses, general robotic uses, and sensing or manipulation of non-tissue work pieces. Other example applications involve surgical or nonsurgical cosmetic improvements, imaging of or gathering data from human or animal anatomy, training medical or non-medical personnel, performing procedures on tissue removed from human or animal anatomies (without return to a human or animal anatomy), and performing procedures on human or animal cadavers.
In the discussion below, boldface lowercase letters (e.g., a) denote column vectors, and boldface upper case letters (e.g., A) denote matrices. The transpose of a matrix A is denoted as AT. The inverse or pseudo inverse of a matrix A is denoted as A−1.
The embodiments below will describe various tools and portions of tools in terms of their state in three-dimensional space. As used herein, the term “position” refers to the location of an object or a portion of an object in a three-dimensional space (e.g., three degrees of translational freedom that can be described using changes in Cartesian X, Y, Z coordinates, such as along Cartesian X, Y, Z axes). As used herein, the term “orientation” refers to the rotational placement of an object or a portion of an object (three degrees of rotational freedom—e.g., which can be described using roll, pitch, and yaw). As used herein, the term “pose” refers to the position of an object or a portion of an object in at least one degree of translational freedom, and to the orientation of that object or that portion of that object in at least one degree of rotational freedom. For an asymmetric, rigid body in a three-dimensional space, a full pose can be described with six parameters in six total degrees of freedom. As used herein, the term “velocity” refers to the first time derivative of pose in general and “acceleration” refers to the second time derivative of pose in general, unless otherwise specified. The term “order” refers to the level of the differentiation with respect to time. For example, velocity is a first order property or signal and acceleration is a second order property or signal. “Higher-order” refers to second or higher order and “lower-order” refers to less than order 2.
As used herein, the term “estimate” of a signal refers to a direct measurement of the signal, or numerical or analytical computation of the signal using different measurements, filtered measurements, and/or a combination thereof. The term “observable” refers to the capability of being estimated. The term “fused” or “fusion” of a plurality of signals refers to combining the plurality of signals, using methods including, for example, arithmetic average, weighted mean, linear or non-linear combination, and Kalman filter with or without the use of additional mathematical models.
Referring to
As shown in
The operator input system 16 for the teleoperational medical system 10 may be “mechanically grounded” by being connected to a base with linkages such as to an operator's console, or it may be “mechanically ungrounded” and not be thus connected. As shown in FIG. 1A, the operator input system 16 is connected to an operator's console 38 that is usually located in the same room as operating table O during a surgical procedure. It should be understood, however, that the operator S can be located in a different room or a completely different building from the patient P. The operator input system 16 generally includes one or more control device(s) for controlling the medical tool 14. (The operator input system 16 is also referred to herein as “master manipulators” and “master input devices” and “input devices”.) The control device(s) may include one or more of any number of a variety of input devices, such as hand grips, joysticks, trackballs, data gloves, trigger-guns, foot pedals, hand-operated controllers, voice recognition devices, touch screens, body motion or presence sensors, and the like. In some embodiments, the control device(s) will be provided with the same degrees of freedom as the medical tools of the robotic assembly to provide the operator with telepresence; that is, the operator is provided with the perception that the control device(s) are integral with the tools so that the operator has a sense of directly controlling tools as if present at the procedure site. In other embodiments, the control device(s) may have more or fewer degrees of freedom than the associated medical tools and still provide the operator with telepresence. In some embodiments, the control device(s) are manual input devices which move with six degrees of freedom, and which may also include an actuatable handle for actuating medical tools (for example, for closing grasping jaw end effectors, applying an electrical potential to an electrode, delivering a medicinal treatment, and the like).
The manipulator assembly 12 supports and manipulates the medical tool 14 while the operator S views the procedure site through the operator's console. An image of the procedure site can be obtained by the medical tool 15, such as via an imaging system comprising a monoscopic or stereoscopic endoscope, which can be manipulated by the manipulator assembly 12 to orient the medical tool 15. An electronics cart can be used to process the images of the procedure site for subsequent display to the operator S through the operator's console. The number of medical tools 14 used at one time will generally depend on the medical diagnostic or treatment (e.g. surgical) procedure and the space constraints within the operating room among other factors. The manipulator assembly 12 may include a kinematic structure of one or more non-servo controlled links (e.g., one or more links that may be manually positioned and locked in place) and a robotic manipulator. The manipulator assembly 12 includes a plurality of motors that drive inputs on the medical tools 14. These motors move in response to commands from the control system (e.g., control system 20). The motors include drive systems which when coupled to the medical tools 14 may advance the medical instrument into a naturally or surgically created anatomical orifice. Other motorized drive systems may move the distal end of the medical instrument in multiple degrees of freedom, which may include three degrees of linear motion (e.g., linear motion along the X, Y, Z Cartesian axes) and in three degrees of rotational motion (e.g., rotation about the X, Y, Z Cartesian axes). Additionally, the motors can be used to actuate an articulable end effector of the tool for grasping tissue in the jaws of a biopsy device or the like. The medical tools 14 may include end effectors having a single working member such as a scalpel, a blunt blade, a needle, an imaging sensor, an optical fiber, an electrode, etc. Other end effectors may include multiple working members, and examples include forceps, graspers, scissors, clip appliers, staplers, bipolar electrocautery instruments, etc.
The teleoperational medical system 10 also includes a control system 20. The control system 20 includes at least one memory 24 and at least one processor 22, and typically a plurality of processors, for effecting control between the medical tool 14, the operator input system 16, and other auxiliary systems 26 which may include, for example, imaging systems, audio systems, fluid delivery systems, display systems, illumination systems, steering control systems, irrigation systems, and/or suction systems. The control system 20 also includes programmed instructions (e.g., a computer-readable medium storing the instructions) to implement some or all of the methods described in accordance with aspects disclosed herein. While control system 20 is shown as a single block in the simplified schematic of
In some embodiments, the control system 20 may include one or more servo controllers that receive force and/or torque feedback from the medical tool 14 or from the manipulator assembly 12. Responsive to the feedback, the servo controllers transmit signals to the operator input system 16. The servo controller(s) may also transmit signals that instruct the manipulator assembly 12 to move the medical tool(s) 14 and/or 15 which extends into an internal procedure site within the patient body via openings in the body. Any suitable conventional or specialized controller may be used. A controller may be separate from, or integrated with, manipulator assembly 12. In some embodiments, the controller and manipulator assembly are provided as part of an integrated system such as a teleoperational arm cart positioned proximate to the patient's body during the medical procedure.
The control system 20 can be coupled to the medical tool 15 and can include a processor to process captured images for subsequent display, such as to an operator using the operator's console or wearing a head-mounted display system, on one or more stationary or movable monitors near the control system, or on another suitable display located locally and/or remotely. For example, where a stereoscopic endoscope is used, the control system 20 can process the captured images to present the operator with coordinated stereo images of the procedure site. Such coordination can include alignment between the stereo images and can include adjusting the stereo working distance of the stereoscopic endoscope.
In alternative embodiments, the robotic system may include more than one manipulator assembly and/or more than one operator input system. The exact number of manipulator assemblies will depend on the surgical procedure and the space constraints within the operating room, among other factors. The operator input systems may be collocated, or they may be positioned in separate locations. Multiple operator input systems allow more than one operator to control one or more manipulator assemblies in various combinations.
The manipulator assembly 12 includes a moveable, lockable, and drivable base 58. The drivable base 58 is connected to a telescoping column 57, which allows for adjustment of the height of the arms 54 (also called “manipulators 54”). The arms 54 may include a rotating joint 55 that both rotates and translates parallel to the column 57. The arms 54 may be connected to an orienting platform 53. The orienting platform 53 may be capable of 360 degrees of rotation. The manipulator assembly 12 may also include a telescoping horizontal cantilever 52 for moving the orienting platform 53 in a horizontal direction.
In the present example, each of the arms 54 includes a manipulator arm portion 51. The manipulator arm portion 51 may connect directly to a medical tool 14. The manipulator arm portion 51 may be teleoperatable. In some examples, the arms 54 connecting to the orienting platform are not teleoperatable. Rather, such arms 54 are positioned as desired before the operator S begins operation with the teleoperative components.
Endoscopic and other imaging systems (e.g., medical tool 15) may be provided in a variety of configurations, including ones having rigid or flexible structures. Rigid endoscopes include a rigid tube housing a relay lens system for transmitting an image from a distal end to a proximal end of the endoscope. Flexible endoscopes transmit images using one or more flexible optical fibers. Digital image based endoscopes may have a “chip on the tip” design in which a distal digital sensor such as a one or more charge-coupled device (CCD) or a complementary metal oxide semiconductor (CMOS) device store image data. Endoscopic imaging systems may also utilize other imaging techniques such as ultrasonic, infrared, and fluoroscopic technologies. Endoscopic imaging systems may provide two- or three-dimensional images to the viewer. Two-dimensional images may provide limited depth perception. Three-dimensional stereo endoscopic images may provide the viewer with more accurate depth perception. Stereo endoscopic tools employ stereo cameras to capture stereo images of the patient anatomy. An endoscopic tool may be a fully sterilizable assembly with the endoscope cable, handle and shaft all rigidly coupled and hermetically sealed.
In some embodiments, the operator input system 16 includes sensor systems 40 located on input control devices 36 and 37. The sensor systems 40 may include one or more sensors including, for example, an inertial measurement unit (IMU), an electromagnetic sensor, a shape sensor, a hybrid sensor system incorporating two or more sensing technologies, other suitable sensor systems, and a combination thereof. An example shape sensor is an optical fiber sensor such as a fiber sensor using Fiber Bragg Grating (FBG) or Raleigh scattering technology. In some examples, the IMU may include an accelerometer configured to measure the linear acceleration of the input control devices 36 and 37, and a gyroscope configured to measure the angular velocity of the input control devices 36 and 37. The sensor systems 40 may provide input control device measurement data (e.g., pose data, acceleration data, and angular velocity data of input control devices 36 and 37) of various state estimates such as position, velocity, and acceleration estimates to the control system 20. In some embodiments, the sensor system 40 is located on an end effector of the input control devices 36 or 37 and provides data for a full 6-DOF acceleration commanded by an operator.
As discussed above, flexibility in a manipulator assembly may cause under-damped vibrations and long settling times. In a system where the manipulator assembly has a relatively large flexibility and/or with relatively large masses and inertias, motions commanded by the operator using an operator input system may cause such vibrations. While performing a medical or non-medical procedure, such vibrations experienced on the manipulator or a tool supported by the manipulator may cause control problems. For example, such vibrations may make it difficult for the computer-assisted system to achieve or follow commanded trajectories. Such vibrations may negatively affect control in all types of robotic systems, including for example, industrial and medical robotic systems. In a medical robotic example, such vibrations may make it difficult for a medical robotic system to perform the commanded manipulations of tissue, movement of imaging systems such as endoscopes and ultrasonic probes, insertion of needles, application of sutures, etc. For a further example, in some embodiments where the manipulator or a tool supported by the manipulator moves about a remote center of motion, the vibrations may cause the manipulator or the tool to move as if the remote center itself has moved from its original or modeled position beyond a predetermined tolerance. As discussed in detail below, by using link sensor systems of the manipulator assembly to provide link data, a master/slave system may control the robotic manipulator assembly such that those vibrations are reduced (e.g. smaller vibration amplitudes, faster settling times, etc.).
Referring to
Referring now to
In some embodiments, the tool 250 may be configured for manipulating industrial work pieces, or to manipulate human or animal tissue for reasons other than medical treatment or diagnosis. In some embodiments, the tool 250 may comprise a tool for performing medical procedures. The tool 250 includes a mounting portion 252 and a shaft 254. In the example shown in
In more detail, the example manipulator 202 includes links L1, L2, L3, and L4 connected by joints J1, J2, J3, and J4 into a kinematic chain. The tool 250's mounting portion 252 is mounted to a carriage 253 located on link L4. Each of the joints J1, J2, J3, and J4 are controlled by motors. The manipulator 202 may also include an insertion assembly that moves the carriage 253 along L4 and provides insertion and withdrawal motion to the tool 250. Other manipulator designs may not have a moveable carriage 253 or may couple with the tool 250 in another manner, and insert and withdraw the tool 250 by moving one or more joints (e.g. joints J2-J4). Accordingly, at least parts of the manipulator assembly 200 are configured to move using motorized or active joints. In this embodiment, the motors of the manipulator 202 are under the control of the control system (e.g., the control system 20) and may be operated in coordination with motors of other manipulator(s) of the same manipulator assembly 200 if the manipulator assembly 200 has other manipulator(s), or in coordination with other manipulator assemblies, to take desired poses that may assist with advancing over a work piece (or a patient in a medical procedure), mounting of tools, preparation steps, storage, moving to target anatomy inside a patient's body and manipulating tissue, placing the remote center of motion, making space for assistants, obstacles, or equipment around the patient, applying forces to anatomical structures such as for palpating tissue, among other activities. In addition, encoders and other sensors associated with each motor or joint of the manipulator assembly 200 provide feedback to the control system so that the control system receives data about, senses or detects, or determines the motion state of the joint/motor, status, torques applied by or on the joints, and setup of the manipulator assembly 200.
In some embodiments, the manipulator assembly 200 is constrained mechanically or by the use of software, such that it moves the tool 250 about a remote center of motion 256 (also called “remote center 256”) during operation. In some embodiments, the remote center of motion 256 is coincident with a shaft of a tool 250 mounted to the manipulator assembly 200. For minimally invasive surgery procedures, typically the position of the remote center of motion 256 is locked at or within the incision in the patient's body wall during at least part of the surgery, during which yaw and pitch motions of the tool 250 about the remote center of motion 256 are allowed to carry out the intended surgical task. Alternatively, the remote center of motion may be located outside of the body to allow a different range of motion. For various procedures, the remote center of motion 256 can be located at any appropriate location for the procedure, such as at a location within a natural orifice or lumen during some medical procedures, a convenient location proximate a work piece during non-medical procedures, etc. Motion about a remote center of motion may be constrained by the use of software (e.g., software of a control system 20) or by a physical constraint defined by a mechanical assembly. In some examples, the procedure may require that the remote center of motion is positioned and maintained within a particular tolerance; that is, the range of location of the virtual fulcrum associated with the motion of the manipulator 202 and/or the tool 250 does not exceed a tolerance amount from a defined remote center of motion. The tolerance may vary, such as varying based on the direction of motion, type of tool, patient habitus, surgical procedure type, etc.
Although each of the joints J1, J2, J3, J4, and carriage 253 may be controlled by an individual or a plurality of joint or actuator controller(s), the joint and actuator controllers may be controlled by a common joint control unit of a common control system (e.g., control system 20, a master/slave control system, etc.). Thus, the tool 250, the tip 262 and end effector 260 of the tool 250, and the manipulator 202 may be controlled through user (e.g., Operator S) manipulation of its associated control device (e.g., the operator input system for the example of
It is noted that the kinematic configuration of the manipulator assembly 200 illustrated in
In the example of
In various embodiments, the manipulator assembly 200 may have different link sensor system arrangements. In the example of
Note that while in
As shown in the example of
As described below with reference to
As described in detail below, in some embodiments, a manipulator assembly may have links without attached link sensor systems. For example, a manipulator assembly including a manipulator having a first number of links may only use a second number of link sensor systems, where the second number is less than the first number. In those embodiments, models (e.g., kinematic models, dynamic models) of the manipulator assembly (including its joints, links, tool and portions (e.g., carriage, end effector) of the tool) are used to supplement the joint and link data to generate joint state estimates. The performance of such a manipulator assembly may be improved by configuring the locations of the link sensor systems based on the models associated with the joints and links. For example, the link sensor systems may be attached to the links of the manipulator that are more different to model, and the rest of the links of the manipulator may not have any link sensor systems.
Referring to
Both the operator input system and slave manipulator may include a number of links connected by joints so as to facilitate multiple degrees-of-freedom movements. As the operator S moves the operator input system from one pose to another during the course of performing a surgical procedure, sensors associated with the operator input system joints provide information indicating such commanded movement in the joint space of the master input device (the “master joint space”). In an example, the commanded movement includes commanded acceleration movement provided by sensors (e.g., sensor systems 40 for the example of
A master input processing unit 301 receives the information of the master joint positions, which are sampled at a control system processing rate, from the master joint encoders in the operator input system, and computes joint velocities from the sensed joint positions. A master forward kinematics processing unit 302 receives the master joint positions and velocities from the master input processing unit 301, and transforms them from master joint space to corresponding positions and velocities of the master frame (i.e., the reference frame associated with the operator input system) in Cartesian space relative to an operator reference frame. In some embodiments, the master forward kinematics processing unit 302 accomplishes this transformation by using a Jacobian and reference frame related information. Example operator reference frames that may be used include the eye reference frame (i.e., the reference frame associated with the position of the operator's eyes). In an embodiment, measurement of Cartesian acceleration may be additionally available from sensor system 40.
A scale and offset processing unit 304 receives the Cartesian pose, velocity, and acceleration commands from the master forward kinematics processing unit 302, scales the commanded movement according to a scale factor selected to perform the procedure, and takes into account offsets to generate desired slave tool frame (i.e., the frame associated with the tool 250) pose and velocities. The scale adjustment is useful where small movements of the slave manipulator 202 of the manipulator assembly 200 are desired relative to the larger movement of the operator input system 16 in order to allow more precise movement of the tool 250 at the procedure site. The offsets, on the other hand, determine, for example, the corresponding position and/or orientation of an end effector frame (e.g., the reference frame associated with an end effector 260 at the distal end of the tool 250) in the camera reference frame (i.e., the reference frame associated with the distal tip of the endoscope) relative to a position and orientation of the master frame in the eye reference frame.
A simulated slave processing unit 308 receives desired slave tool frame position and velocity commands from the scale and offset processing unit 304, and limits the desired slave tool frame state (e.g., pose, velocities, and accelerations) to assigned Cartesian limits for instance to enforce correct and intuitive operation of the tool 250 by keeping it within its dexterous workspace. The simulated slave processing unit 308 generates simulated slave joint states (e.g., positions, velocities, accelerations) corresponding to the limited slave tool frame state, while making sure that the generated slave joint states do not exceed the actual slave joint's range of motion (e.g., velocities and accelerations) and maximum positions even in the vicinity of kinematic singularities for the slave kinematics. In an example, the simulated slave joint states are determined based on a manipulator Jacobian according to the following equations:
{dot over (q)}=J
−1
{dot over (x)}; (i)
{umlaut over (q)}=J
−1({umlaut over (x)}−{dot over (J)}{dot over (q)}); (ii)
where {dot over (x)} is the slave tool velocity, {umlaut over (x)} is the slave tool acceleration, J is the Jacobian of the manipulator assembly, {dot over (q)} is the slave joint velocity, and {umlaut over (q)} is the salve joint acceleration.
An inverse scale and offset processing unit 306 receives the simulated joint position and velocity commands from the simulated slave processing unit 308, and performs an inverse function (inverse to that of the scale and offset processing unit 304) on them. A Cartesian controller 307 receives the inputs to the scale and offset processing unit 304 as first inputs, and the outputs of the inverse scale and offset processing unit 306 as second inputs. The Cartesian controller 307 then generates an error signal as a difference between the first and second inputs, and a Cartesian force “FCART” from the error signal such as with the following formula:
F
CART
=K(Δx)+B(Δ{dot over (x)})+M(Δ{umlaut over (x)}) (1)
where “K” is a spring constant, “B” is a damping constant, M is a mass constant, “Δ{dot over (X)}” is the difference between the Cartesian velocity inputs to the Cartesian controller 307, “Δx” is the difference between the Cartesian position inputs to the Cartesian controller 307, and “Δ{umlaut over (x)}” is the difference between the Cartesian acceleration inputs to the Cartesian controller 307.
A master transpose kinematics processing unit 315 receives the Cartesian force FCART through a summation node 314, and generates a corresponding torque in joint space using, for example, the Jacobian transpose matrix and kinematic relationships associated with the operator input system. In systems where the operator input system has motor-driven joints for range-of-motion limits or force feedback, a master output processing unit 316 receives the master torque signals from the master transpose kinematics processing unit 315, generates electrical currents corresponding to the master torque signals, and supplies the electrical currents to corresponding master joint motors of the operator input system (e.g. the operator input system 16 of the example of
As the master input processing unit 301 is receiving master joint positions from sensors in the operator input system, a slave input processing unit 309 is also receiving slave positions from sensors in the slave manipulator 202 at the control system processing rate. The slave input processing unit 309 includes a motor side input processing unit 320 receiving slave joint measurement data (e.g., slave joint state data including slave joint pose, velocity, and acceleration data) from motor side sensors (e.g., joint encoders), and a load side (e.g., link side) input processing unit 322 receiving slave link data (e.g., position and motion data of the links L1, L2, L3, and L4 for the example of
The slave torque command signals are generated by the joint control unit 318 so as to drive joints of the slave manipulator until feedback errors calculated in the joint control unit 318 zero out. A slave output processing unit 310 receives the slave torque command signals from the joint control unit 318, converts them into appropriate electrical currents, and supplies the electrical currents to the joint motors of the slave manipulator so as to drive the motors accordingly.
In some embodiments, the master torque feedback command signals are generated by the joint control unit 318 which reflect forces being exerted against the tool 250, or against the slave manipulator supporting the tool 250, back to the operator input system (e.g. the operator input system 16) so that they may be felt in some form by the operator S. In some embodiments, the joint control unit 318 generates the master torque feedback command signals based on the slave joint position and velocity tracking errors. In some embodiments, the joint control unit 318 generates the master torque feedback command signals based on measurement data from both the slave joint encoders and the link sensor system 204, which is described in detail below. A kinematic mapping unit 311 receives the master torque feedback command signals from the joint control unit 318, and generates the corresponding Cartesian force at the tip of the tool 250 relative to the camera frame of the endoscope using the slave kinematic configuration and the previously calculated slave fulcrum (e.g., remote center 256) position information.
A gain 313 adjusts the magnitude of the Cartesian force so as to ensure system stability while providing adequate force sensation to the operator S. The gain adjusted Cartesian force is then passed through the summation node 314, and processed along with the Cartesian force provided by the Cartesian controller 307 through the master transpose kinematics processing unit 315 and master output processing 316 as previously described in reference to their processing of the Cartesian force provided by the Cartesian controller 307.
The joint control unit 318 includes a joint controller for each active joint of the slave manipulator 202 of the manipulator assembly 200 that is being controlled by the master/slave control system 300. In particular, where the slave manipulator 202 includes joints J1, J2, J3, J4, and a carriage 253 moveable along the link L4, such as the slave manipulator 202 for the examples of
Referring to
In the example of
In some embodiments, the desired joint state data 402 Θd may include a desired joint position θd, a desired joint velocity {dot over (θ)}d, and a desired joint acceleration {umlaut over (θ)}d. In an example, the desired joint acceleration {umlaut over (θ)}d is generated based on operator input acceleration measurement data provided by an acceleration sensor in the operator input system (e.g., sensor 40 operator input system 16 of
In some embodiments, the actual joint state estimates 406 {tilde over (Θ)}act include an actual joint pose estimate {tilde over (θ)}act, an actual joint velocity estimate {tilde over ({dot over (θ)})}act, and an actual joint acceleration estimate {tilde over ({umlaut over (θ)})}act. The comparator 404 compares the desired joint state data 402 Θd and actual joint state estimates 406 {tilde over (Θ)}act to generate joint feedback errors 410, denoted as Θerr. The joint feedback errors Θerr may include a joint position feedback error θerr including a difference between the desired joint position θd and actual joint pose estimate {tilde over (θ)}act, a joint velocity feedback error θerr including a difference between the desired joint velocity {dot over (θ)}d and actual joint velocity estimate {tilde over ({dot over (θ)})}act, and a joint acceleration feedback error {umlaut over (θ)}err including a difference between the desired joint acceleration {umlaut over (θ)}d and actual joint acceleration estimate {tilde over ({umlaut over (θ)})}act.
The comparator 404 may send join feedback errors 410 Θerr to a feedback controller 412. The feedback controller 412 may include an acceleration term (denoted as A) for the joint acceleration feedback error of the join feedback errors 410 Θerr. In an example, the feedback controller 412 includes a proportional-derivative-acceleration (PDA) controller. In another example, the feedback controller 412 includes a proportional-integral-derivative-acceleration (PIDA) controller having a closed loop transfer function in the Laplace domain as follows:
where Ka is the acceleration gain, Kd is the derivative gain, Ki is the integral gain, Kp is the proportional gain, and s is the complex frequency. Ka, Kd, Ki, and Kp are tunable controller parameters, and may be tuned and configured together. By using an acceleration error term, such as a term generated using joint state estimate and the acceleration signal in an appropriate frame from sensor system 40, the performance (e.g., controlled damping, settling time, tracking error) of the feedback controller 412 is improved. In an example, in response to a step disturbance (e.g., a step disturbance detected and transmitted to the controller, such as due to a physical tap on one or more links of the manipulator 202 while the manipulator 202 is moving to a pre-defined pose, there is about 50% reduction in setting time and increased controlled damping behavior compared to a feedback controller without an acceleration term.
In some embodiments, the feedback controller 412 may be designed by interpreting Ka as electronic inertia. In such embodiments, the PDA form of feedback controller 412 is considered to result in a closed loop transfer function (CLTF(s)) for a simple spring mass damper system as follows:
CLTF(s)=Kas2+Kds+Kp/[(m+Ka)s2+(b+Kd)s+(k+Kp)], (3)
where Ka is the acceleration gain, Kd is the derivative gain, Kp is the proportional gain, s is the complex frequency, m, b, k are the manipulator joint's natural (or open-loop) parameters for mass, damping, and stiffness respectively. In an example, Ka is determined based on apparent mass (m+Ka), Kd is determined based on closed loop damping (b+Kd), and Kp is determined based on servo stiffness (k+Kp). As such, the closed loop performance of the feedback controller 412 may be specified in terms of a bandwidth determined by the apparent mass and servo stiffness, together with closed loop damping ratio (determined by closed loop damping).
As illustrated in
In some embodiments, the slave manipulator 202 includes joint encoders 422 providing joint encoder data 424 (denoted as Θenc) of the joints J1, J2, J3, and J4 of
In some embodiments, a joint state estimator 430 receives the joint encoder data 424 Θenc and link measurement data 426
and generates joint state estimates 432. As discussed in detail below, the joint state estimator 430 may apply different joint state estimation processes based on various link sensor system arrangements (e.g., whether each of the adjacent links has a link sensor system, whether a single link has more than one link sensor systems) and joint state estimation settings (e.g., whether to provide a fused joint state estimate). The joint state estimator 430 may include various estimation blocks, including for example, a recursive kinematics estimator 440, a fused estimator 441, a model-based bridging estimator 442 (also referred to as a bridging estimator 442), a redundant sensing estimator 428, and any other suitable estimation blocks for performing joint state estimation. Specifically, the recursive kinematics estimator 440 uses a kinematic model to generate joint state estimates 432 using the link data. The bridging estimator 442 uses models (e.g., a dynamic model) of the manipulator joints and links to supplement the link data in generating the joint state estimates 432, for example, when a link sensor system 204 is not present on at least one link adjacent to the joint.
Referring to
Referring to
In an example, the recursive kinematics estimator 440 may perform the joint state (e.g., velocity and acceleration) estimation based on a kinematic model using the Denavit and Hartenberg (DH) convention. The DH convention uses kinematic parameters (also referred to as DH parameters) including joint angle θi, link offset di, link length ai, and link twist αi corresponding to one degree of freedom. These kinematic parameters may be used to describe the translation and rotation between two adjacent links Li and Li+1 coupled at a 1-DOF joint Ji+1. In some embodiments, because the recursive kinematics estimator 440 relies on the incremental differential kinematics between two adjacent links, the kinematic parameters in the kinematics model defining these differential kinematics are calibrated to ensure the accuracy of the recursive kinematics estimator 440.
The kinematics between two adjacent links Li and Li+1 may be represented by the following equations:
ωi+1=ωi+Σnλn(RDH
ωi+1=ωi+Σnλn(RDH
{dot over (x)}
i+1
={dot over (x)}
1+ωi×ri+ωi+1×ri+1+Σn(1−λn)(RDH
{umlaut over (x)}
i+1
={umlaut over (x)}
i+{dot over (ω)}i×ri+ω1×ωi×ri+{dot over (ω)}i+1×ri+1+ωi+1×ωi+1×ri+1+Σn(1−λn)(RDH
In those equations, x represents the vector cross product operator, n is the joint axis index (e.g., 1, 2, 3) belonging to the joint Ji+1. {circumflex over (Z)}n is the nth joint axis. {dot over (θ)}n is the joint velocity associated with (e.g., projected on) the nth joint axis. {umlaut over (θ)}n is the joint acceleration associated with (e.g., projected on) the nth joint axis. RDH
Based on equations (4) through (7), the recursive kinematics estimator 440 may perform joint state estimation based on recursive kinematics. Referring to
The method 550 begins at operation 552, where the recursive kinematics estimator 440 receives link measurement data 426 from link sensor systems 204 located on links of a manipulator 202. The link measurement data 426 may include link angular velocity data or link acceleration data. The method 550 may proceed to operation 554 to set iteration index i to zero. The method 550 may then proceed to operation 556, where ω0 is estimated at the ground or base link L0. In an example, the recursive kinematics estimator 440 may set ω0 to have a value zero. In such an example, the link L0 may be a link in the chain which does not move dynamically (e.g., a link configured to be moved slowly, or set in a position, locked, and not moved again during a particular operation). Such a link L0 may be a setup structure or setup joints of the manipulator assembly or a vertical column at a base of the manipulator assembly. In another example where a base link L0 has redundant (more than one) sensors, ω0 may be computed. Such computation may include numerically differentiating the measurement data of the redundant sensors and incorporating a filter (e.g., a Kalman filter).
The method 550 may then proceed to operation 558 to estimate the joint velocity {dot over (θ)}n for joint Ji+1 based on link measurement data ωi provided by the link sensor system 204 on the link Li and link measurement data ωi+1 provided by the link sensor system 204 on the link Li+1. In an example, at operation 558, the recursive kinematics estimator 440 may estimate the joint velocity {dot over (θ)}n according to equation (4) as follows:
{dot over (θ)}n=(ωi+1−ωi)·(RDH
As shown in equation (8), the differential angular velocity Δω of adjacent links Li and Li+1 may be computed using ωi and ωi+1. The joint velocity {dot over (θ)}n for joint Ji+1 may then be estimated by transforming the differential angular velocity Δω to the joint space by applying RDH
The method 550 may then proceed to operation 560, where the recursive kinematics estimator 440 may compute an estimation for angular link acceleration {dot over (ω)}i+1 for link Li+1 based on link measurement data ωi, ωi+1, {umlaut over (x)}i, {umlaut over (x)}i+1 and estimated link data {dot over (ω)}i according to equations (7) as follows:
ωi+1=(ri+1×)−1(−({umlaut over (x)}i+1−{umlaut over (x)}i)+ωi×ri+ωi×ωi×ri+ωi+1×ωi+1×ri+1). (9)
In the ith iteration where i=0, {dot over (ω)}1 is computed based the estimated angular link acceleration {dot over (ω)}0 generated at operation 556. In the ith iteration where i is greater than 0, {dot over (ω)}i+1 is computed based the estimated angular link acceleration {dot over (ω)}i generated at operation 560 in the previous (i−1)th iteration.
The method 550 may then proceed to operation 562, where the recursive kinematics estimator 440 computes joint acceleration {umlaut over (θ)}n for joint Ji+1 using estimated link data {dot over (ω)}i and {dot over (ω)}i+1 and link measurement data ωi as follows:
{umlaut over (θ)}n=({dot over (ω)}i+1−{dot over (ω)}i)·RDH
In some embodiments, the recursive kinematics estimator 440 may perform kinematic estimation for {dot over (x)}i+1 according to equation (6) using {dot over (x)}i, ωi, ri, ωi+1, ri+1, RDH
The method 550 may then proceed to operation 564, where the recursive kinematics estimator 440 determines whether the joint Ji+1 is the last joint of the manipulator assembly for joint state estimation. In some embodiments, the recursive kinematics estimator 440 determines that the joint Ji+1 is not the last joint, and the method 550 continues to operation 556 to increase in the iteration index i by one, and then performs the next iteration including operations 558 through 564 to perform joint state estimation for the next joint in the serial chain.
In some embodiments, at operation 564, the recursive kinematics estimator 440 determines that the joint Ji+1 is the last joint for joint state estimation. The method 550 may then proceed to block 568, where the recursive kinematics estimator 440 outputs the joint state estimates 432.
As discussed above, the joint state estimates 432 (e.g., {dot over (θ)}n, {umlaut over (θ)}n) may be determined based on the link measurement data and the rotation matrix RDH
Referring to
Referring to the example of
In some embodiments, a first joint acceleration estimator 610 of the recursive kinematics estimator 440 receives the joint velocity estimates 608 from the joint velocity estimator 606, and generates a first joint acceleration estimate 612 by based on the joint velocity estimates 608. As such, the first joint acceleration estimate 612 is generated only based on link angular velocity data ωi and ωi+1 without using link acceleration data {umlaut over (x)}i and {umlaut over (x)}i+1. In some examples, the first joint acceleration estimate 612.
In some embodiments, a fusion joint acceleration estimator 618 of the recursive kinematics estimator 440 receives first joint acceleration estimate 612 from the first joint acceleration estimator 610, and receives second joint acceleration estimate 616 from a second joint acceleration estimator 614. The second joint acceleration estimator 614 may generate joint acceleration estimate 616 using link acceleration data form either adjacent links to the joint or one or more links that are connected to the joint. The fusion joint acceleration estimator 618 may generate a fused joint acceleration estimate 620 by fusing the first joint acceleration estimate 612 and second joint acceleration estimate 616. Because fused joint acceleration estimate 620 is generated based the first joint acceleration estimate 612 and second joint acceleration estimate 616 generated based on different data and/or different estimation models, it may improve the robustness and SNR of the fused estimate.
Referring to
{dot over (ω)}i=−(di×)−1({umlaut over (x)}link_sensor_2−{umlaut over (x)}link_sensor_1), (11)
where {umlaut over (x)}link_sensor_1 and {umlaut over (x)}link_sensor_2 are the translational acceleration link data from redundant link sensor systems 204-1 and 204-2 respectively, ({umlaut over (x)}link_sensor_2−{umlaut over (x)}link_sensor_1) is the vector difference between translational accelerations of the redundant link sensor systems 204-1 and 204-2. The joint acceleration {umlaut over (θ)}n for joint Ji+1 may then be computed based on equation (10).
Referring to
Referring to the example of
In some embodiments, the bridging estimator 442 may generate joint state estimates 432 of a particular joint (e.g. joint J2) by fusing intermediate joint state estimates of that particular joint. Each intermediate estimate is generated based on link data from a single link sensor system 204 associated with a particular link. In an example, the particular link (e.g., link L2) is connected to the particular joint J2 and has a link sensor system 204. In another example, the particular link (e.g., link L4) is not connected to the particular joint J2 and has a link sensor system 204. In yet another example, neither of the adjacent links connected to that particular joint has a link sensor system. In that example, all intermediate estimates for that particular joint are generated using link data associated with links not connected to that particular joint.
In some embodiments, the translational acceleration data xi and angular velocity data ωi of a link Li may be expressed as follows:
{umlaut over (x)}
i
=J
t
{umlaut over (θ)}+{dot over (J)}
t{dot over (θ)}, (12)
ωi=Jωθ, (13)
where Jt is the translational Jacobian of the link sensor system 204 on link Li in the world reference frame W, Jω is the orientational Jacobian of the link sensor system 204 on link Li in the world reference frame W, {dot over (θ)} and {umlaut over (θ)} are the joint velocities and joint accelerations of joints of the manipulator respectively.
Using equations (11) and (12), the bridging estimator 442 may determine the joint velocity and acceleration estimates of the joints of the manipulator based on link sensor data from a particular link sensor system 204 on a link Li as follows:
{dot over (θ)}link_sensor_i=Jω−1ωi, (14)
{dot over (θ)}link_sensor_i=Jt−1({umlaut over (x)}i−{dot over (J)}tJω−1ωi), (15)
where {dot over (θ)}link_sensor_i denotes the estimates for joint velocities of joints of the manipulator based on the link measurement data of link Li, and {umlaut over (θ)}link_sensor_i denotes the estimates for joint accelerations of joints of the manipulator based on the link measurement data of link Li.
Referring to
{dot over (θ)}=Σi=1Mweight1i{dot over (θ)}link_sensor_i, (16)
{dot over (θ)}=Σi=1Mweight2i{umlaut over (θ)}link_sensor_i, (17)
where weight1i is the weight assigned to intermediate estimate {dot over (θ)}link_sensor_i generated using link data from the link sensor system 204 on link Li, weight2i is the weight assigned to intermediate estimate {umlaut over (θ)}link_sensor_i generated using link data from the link sensor system 204 on link Li, and M is an integer between 1 and N, N being the total number of available link sensor systems 204. Because the joint state estimates depend on Jacobian Jt and Jω determined based on kinematic models of the manipulator, the accuracy of joint state estimates may depend on the accuracy of kinematic calibration of the manipulator.
In the example of
In some embodiments, the bridging estimator 442 may apply different weights to the first and second intermediate estimates for joint J2. The weights may be based on the distance between the associated link sensor system 204 and a control point (e.g., a tool tip 262). In an example, a weight applied to the second intermediate estimate is greater than the weight applied to the first intermediate estimate, where the second intermediate estimate is associated with the link sensor system 204 of link L4 that is closer to a control point (e.g., a tool tip 262). The weighted first and second intermediate estimates for joint J2 may then be fused to generate the joint state estimate 432 for joint J2.
Note that in the example of
Referring to
{umlaut over (θ)}(n1:n2)=Jt_section−1(Δ{umlaut over (x)}section−{dot over (J)}t_sectionJω_section−1Δωsection), (18)
where Jt_section is translational Jacobian describing the link sensor system 204 on link Ln2 with regard to the link sensor system 204 on link Ln1−1, where Jω_section is the orientational Jacobian describing the link sensor system 204 on link Ln2 with regard to the link sensor system 204 on link Ln1−1. Jt_section and Jω_section may be determined based on kinematic models of the section (e.g., including links Ln1−1, joints Jn1 through Jn2 and the intermediate links thereof, and link Ln2) of the manipulator, as such, their accuracy depend on the accuracy of kinematic calibration of the manipulator. Δ{umlaut over (x)}section is the differential translational acceleration of the link sensor system 204 on link Ln2 with regard to the link sensor system 204 on link Ln1−1, and may be computed using {umlaut over (x)}n2 and {umlaut over (x)}n1−1 of the link data from the link sensor systems 204 on links Ln2 and Ln1−1. Δωsection is the differential angular velocity of the link sensor system 204 on link Ln2 with regard to the link sensor system 204 on link Ln1−1, and may be computed using ωn2 and to ωn1−1 of the link data from the link sensor systems 204 on links Ln2 and Ln1−1.
In the example of
Referring to
In some examples, the joint dynamic model may be generated based on various dynamic properties of the manipulator 202, including, for example, mass properties of the links, transmission stiffness, friction, damping, and other dynamic properties. The bridging estimator 442 may compute joint state (e.g., velocity, acceleration) estimates for joint J1 based on the joint dynamic model. Specifically, the bridging estimator 442 may compute joint state estimates based on the torque applied to the associated joint motor in a previous time step by solving the forward dynamics for joint J1.
In some embodiments, the bridging estimator 442 may also use the dynamic models of the joints J1 and J2 to bridge the information gap of L1 caused by the lack of a link sensor system thereon for the recursive kinematics estimator 440. As illustrated in the example of
After the fictitious link sensor system 702 of link L1 is provided by the bridging estimator 442, the recursive kinematics estimator 440 may generate a joint state estimate for joint J2 using the link data of the link L1 (from the fictitious link sensor system 702) and link L2 (from the link sensor system 204) as discussed above based on equations (8), (9), and (10). Specifically, the recursive kinematics estimator 440 may generate a differential motion of adjacent links L1 and L2 based on the link data of the link L1 (from the fictitious link sensor system 702) and link L2 (from the link sensor system 204), and generate the joint state estimate for joint J2 by transforming that differential motion to the joint space of joint J2.
In some embodiments, the link state estimate of the link L1 (e.g., link data provided by the fictitious link sensor system 702) may be determined based on combining a forward link state estimate of the link L1 and a backward link state estimate of the link L1. For example, a forward link state estimate of the link L1 may be determined by forward propagating kinematics based on the link data of the link L0 and an initial joint state estimate of the joint J1. The initial joint state estimate of the joint J1 may be computed by solving forward dynamics based on a force on the joint J1 and a dynamic model of the joint J1. For further example, a backward link state estimate of the link L1 may be determined by backward propagating kinematics based on the link data of the link L2 and an initial joint state estimate of the joint J2. The initial joint state estimate of the joint J2 may be computed by solving forward dynamics based on a force on the joint J2 and a dynamic model of the joint J2.
In some embodiments, the joint state estimates of the joints J1 and J2 may be updated based on the link state estimate of the link L1 (e.g., determined based on the initial joint state estimates of the joints J1 and J2). For example, a joint state estimate for the joint J1 may be determined based on the link state estimate of the link L1 and the link data of the link L0. For further example, a joint state estimate for the joint J2 may be determined based on the link state estimate of the link L1 and the link data of the link L2.
In those embodiments, because the bridging estimator 442 uses the dynamic model to compute joint state estimates, control system performance may be improved by providing a more accurate dynamic model. Accordingly, in embodiments where the number of link sensor systems is less than the total number of links of a manipulator, the performance of the control system is improved by configuring the locations of the link sensor systems based on the difficulty of modeling the links and joints of the manipulator 202. In an example, the manipulator 202 may be configured to have the link sensor systems attached to the links or joints that are more difficult to model.
Referring to
In some embodiments, the weighting block 434 includes an inertia block 444 that applies weights based on corresponding joint inertia. For a given control point (e.g., a tool tip 262 or a remote center 256), the inertia block 444 applies weights to the joint state estimates 432 based on the corresponding joint inertias. In some embodiments, the joint inertias are dependent on the configuration of the manipulator assembly 200. As such, the inertia block 444 may dynamically adjust the weights based on the configuration of the manipulator assembly 200. For example, the inertia block 444 receives first and second joint state estimates associated with first and second joints respectively, and applies first and second weights to the first and second joint state estimates respectively. In that example, the first weight is less than the second weight because the joint inertia of the first joint is less than that of the second joint. In various embodiments, the relationship between the weights and the corresponding joint inertias may be linear or nonlinear.
In some embodiments, the weighting block 434 includes a physical compliance block 446. For a given control point (e.g., a tool tip 262 or a remote center 256), the physical compliance block 446 apply weights to joint state estimates 432 based on the corresponding joint physical compliances. As such, the physical compliance block 446 may change the weights based on the configuration of the manipulator assembly 200. For example, the physical compliance block 446 receives first and second joint state estimates associated with first and second joints respectively, and applies first and second weights to the first and second joint state estimates respectively. In that example, the first weight is less than the second weight because the physical compliance of the first joint is less than that of the second joint. In various embodiments, the relationship between the weights and the corresponding physical compliances may be linear or nonlinear.
In various embodiments, the weighting block 434 may generate the weights based on the inertia, compliance, and damping of a portion of or the entire manipulator assembly, including the joints, links, and the tool. A modal weighting block 450 of the weighting block 434 may identify modal parameters (e.g., vibration frequencies and damping ratios of various vibrational modes) of the manipulator assembly (e.g., using experimental modal analysis). The modal weighting block 450 may determine the modal mass, stiffness, and damping, and then transform them to the joint actuator/joint space. The modal weighting block 450 may determine the contribution of each joint to a given vibrational mode (e.g., using a modal matrix), and determine the weights based on the contribution of a given joint to a particular vibrational mode (e.g., a fundamental vibrational mode). In an example, a larger weight is assigned to joint state estimates of a joint that has a higher contribution to a particular vibrational mode. Below is an example of a modal matrix Ψ for a 4-DOF manipulator system with 4 vibrational modes, where rows one through four corresponds to joints J1, J2, J3, and J4 (and their DOFs) respectively, and each column corresponds to a vibrational mode:
In an example, the chosen mode corresponds to column 1 (e.g., a fundamental mode) of the modal matrix Ψ, then the weight for joint J1 with the 1st DOF is higher than the weights for other joints because the corresponding matrix element (“1.0”) has the largest absolute value. In another example where the chosen mode corresponds to column 4, the weight for joint J3 with the 3rd DOF is higher than the weights for the other joints because the corresponding matrix element (“−3.9”) has the largest absolute value. In a particular example, the weighting block 434 may use a scaled version of the normalized Eigenvector of the modal matrix Ψ as the weight vector for the corresponding joints.
As illustrated in
As shown in the method 800, a joint controller may control the movement of the joints of a manipulator based on joint acceleration estimates of joints of a manipulator. By using such joint acceleration estimates, the manipulator assembly is more responsive to acceleration transients (e.g., caused by link vibrations). To further reduce the vibrations at a given control point (e.g., a tool tip, a remote center), various weighting schemes may be applied to the joint acceleration estimations (e.g., based on joint inertias, joint physical compliances, and joint distances to a control point). Furthermore, the joint acceleration estimates are generated based on link angular velocity data, link acceleration data, or a combination thereof. In an example, by using a fused joint acceleration estimate generated based on both link angular velocity and acceleration data, robustness and signal-to-noise ratio (SNR) of the fused signal may be improved.
The method 800 begins at process 802, where a joint state estimator receives link data from link sensor systems located on links of a manipulator. In the example of
The method 800 may then proceed to process 804, where the joint state estimator generates joint state (e.g., pose, velocity, acceleration) estimates based on the link data on adjacent links. In the example of
At process 804, in examples where each of the adjacent links coupled to a particular joint has a link sensor system, a recursive kinematics estimator 440 of the joint state estimator 430 may generate the joint state estimates 432 of that particular joint based on the link data based on recursive kinematics as described above with reference to
At process 804, in examples where at least one of adjacent links coupled to a particular joint does not have a link sensor system, a bridging estimator 442 of the joint state estimator 430 may generate the joint state estimates 432 of that particular joint based on the link data of links not connected to that particular joint. In an example, the bridging estimator 442 of the joint state estimator 430 generates the joint state estimates 432 using a joint dynamic model of that particular joint.
The method 800 may proceed to process 806, where weights are applied to the joint state estimates to generate weighted joint state estimates. In the example of
The method 800 may then proceed to block 808, where the motion of the joints is controlled based on the weighted joint state estimates. In the example of
Referring to
In some embodiments, the torque adjustment 908 is applied to a slave torque command signal 414 to generate an adjusted slave torque command signal 420 for controlling the joints of the manipulator 202. In the example of
Referring to
As shown in the example of
In some embodiments, residual accelerations at particular joints may not be controlled locally by joint actuators of these particular joints. For example, a residual acceleration R21 at the joint J2 along the axis 1002 is not controlled locally by a joint actuator of the joint J2. For further example, a residual acceleration R31 at the joint J3 along an axis 1004 aligned with the axis 1002 is not controlled locally by a joint actuator of the joint J3.
In some embodiments, a joint controller (e.g., a joint control unit 400 of
One or more elements in embodiments of the invention may be implemented in software to execute on a processor of a computer system such as control processing system. When implemented in software, the elements of the embodiments of the invention are essentially the code segments to perform the necessary tasks. The program or code segments can be stored in a processor-readable storage medium or device that may have been downloaded by way of a computer data signal embodied in a carrier wave over a transmission medium or a communication link. The processor readable storage device may include any medium that can store information including an optical medium, semiconductor medium, and magnetic medium. Processor readable storage device examples include an electronic circuit; a semiconductor device, a semiconductor memory device, a read-only memory (ROM), a flash memory, an erasable programmable read-only memory (EPROM); a floppy diskette, a CD-ROM, an optical disk, a hard disk, or other storage device, the code segments may be downloaded via computer networks such as the Internet, Intranet, etc.
Note that the processes and displays presented may not inherently be related to any particular computer or other apparatus. Various general-purpose systems may be used with programs in accordance with the teachings herein, or it may prove convenient to construct a more specialized apparatus to perform the operations described. The required structure for a variety of these systems will appear as elements in the claims. In addition, the embodiments of the invention are not described with reference to any particular programming language. It will be appreciated that a variety of programming languages may be used to implement the teachings of the invention as described herein.
While certain exemplary embodiments of the invention have been described and shown in the accompanying drawings, it is to be understood that such embodiments are merely illustrative of and not restrictive on the broad invention, and that the embodiments of the invention not be limited to the specific constructions and arrangements shown and described, since various other modifications may occur to those ordinarily skilled in the art.
This application claims the benefit of U.S. Provisional Application 62/584,206 filed Nov. 10, 2017, which is incorporated by reference herein in its entirety.
| Number | Date | Country | |
|---|---|---|---|
| 62584206 | Nov 2017 | US |
| Number | Date | Country | |
|---|---|---|---|
| Parent | 16185687 | Nov 2018 | US |
| Child | 17493918 | US |
