Method and system for constraint-aware inverse kinematics with trajectory planning for a surgical robot

Abstract

A method of a surgical robotic system that includes a human interface device (HID) configured to control a robotic arm that includes several joints, the method includes receiving, via the HID, a target pose of an end effector of the robotic arm. The method determines a trajectory for moving the joints to cause the end effector to move to the target pose, where the trajectory includes a series of joint movements, each movement satisfying a unified constraint of the joint associated with a position of the joint form which the joint movement is to occur, and outputting the trajectory to cause the robotic arm to move the end effector into the target pose.

Description

Atorney Docket No. : AUR6374WOPCT2Electronically FiledMethod and System for Constraint-Aware Inverse Kinematics with Trajectory Planning for a Surgical RobotRELATED APPLICATIONS

[0001] This application claims the benefit of priority of U.S. Provisional PatentApplication No. 63 / 707,181, filed October 14, 2024 and U.S. Provisional Patent Application No. 63 / 707,187, filed October 14, 2024, which are herein incorporated by reference in their entirety.FIELD

[0002] Various embodiments of the disclosure relate generally to surgical systems, and more specifically to a surgical system for constraint-aware inverse kinematics with trajectory planning for surgical robot movement. Other embodiments are also described.BACKGROUND

[0003] Minimally-invasive surgery, MIS, such as laparoscopic surgery, uses techniques that are intended to reduce tissue damage during a surgical procedure. Laparoscopic procedures typically call for creating a number of small incisions in the patient, e.g., in the abdomen, through which several surgical tools such as an endoscope, a blade, a grasper, and a needle, are then inserted into the patient. A gas is injected into the abdomen which insufflates the abdomen thereby providing more space around the tips of the tools, making it easier for the surgeon to see (via the endoscope) and manipulate tissue at the surgical site. MIS can be performed faster and with less surgeon fatigue using a surgical robotic system in which the surgical tools are operatively attached to the distal ends of robotic arms, and a control system actuates the arm and its attached tool. The tip of the tool will mimic the position and orientation movements of an input device as the latter is being manipulated by the surgeon. The surgical robotic system may have multiple surgical arms, one or more of which has an attached endoscope, and others have attached surgical instruments for performing certain surgical actions.Atorney Docket No. : AUR6374WOPCT2Electronically FiledSUMMARY

[0004] According to one embodiment of the disclosure, a method of a surgical robotic system that includes a human interface device (HID) configured to control a robotic arm that includes several joints, the method including: receiving, via the HID, a target pose of an end effector of the robotic arm; determining a trajectory for moving the joints to cause the end effector to move to the target pose, where the trajectory includes a series of joint movements, each joint movement satisfying a unified constraint of the joint associated with a position of the joint from which the joint movement is to occur; and outputting the trajectory to cause the robotic arm to move the end effector into the target pose.

[0005] In one embodiment, the method further including: determining an intermediate pose of the end effector between a starting pose of the end effector and the target pose of the end effector; and determining, for each joint, a target joint velocity for moving from a current joint position, where determining the trajectory includes determining, for each joint, a joint velocity for the joint movement based on the target joint velocity that satisfies the unified constraint for moving the end effector from the starting pose into the intermediate pose. In another embodiment, determining the trajectory includes: determining, for each joint, whether the joint velocity of the joint is to cause the joint to exceed at least one of a joint position limit, a joint velocity limit, or a joint acceleration limit; and, in response, adjusting a respective joint movement such that the respective joint is to stop at or before at least one of the joint position limit, the joint velocity limit, or the joint acceleration limit when the trajectory is output. In some embodiments, determining the intermediate pose of the end effector includes: determining a pose error based on a difference between the starting pose and the target pose of the end effector; and applying a gain to the pose error based on the difference. In another embodiment, the method further including determining, for each joint, the unified constraint based on the current joint position, where determining the joint velocity for each joint includes: determining a scaling factor that enables unified motion across the joints based on all unified constraints of the joints; and applying the scaling factor to the target joint velocity of the joint.Atorney Docket No. : AUR6374WOPCT2Electronically Filed

[0006] In one embodiment, unified constraint accounts for at least one of a position limit of the joint, a velocity limit of the joint, or an acceleration limit of the joint at the current joint position. In another embodiment, the method further including: receiving position data of the joints; updating the position data of the joints based on the joint movements; and determining a resulting pose of the end effector based on the updated position data, where the trajectory is output responsive to a determination that the resulting pose of the end effector converges with the target pose of the end effector within a threshold. In another embodiment, the method of claim 7 further including iteratively, until the resulting pose of the end effector converges with the target pose of the end effector, determining an intermediate pose of the end effector between a previously determined resulting pose and the target pose of the end effector; determining, for each joint, a new joint movement based on a new target velocity that satisfies a new unified constraint for a current joint position of the joint; and determining the new resulting pose of the end effector based on the determined new joint movements. In another embodiment, determining the trajectory includes: for each joint, determining whether the joint movement is to cause the joint to reach its respective joint position limit; and responsive to determining that a particular joint movement is to cause a respective joint to reach its respective joint position limit, adjusting the particular joint movement to cause the respective joint to stop at its respective joint position limit while still satisfying the unified constraint.

[0007] According to another embodiment of the disclosure, a surgical system including: a robotic arm that includes several joints; a HID configured to control the robotic arm; at least one processor; and memory having instructions stored therein which when executed by the at least one processor causes the surgical system to: receive, via the HID, a target pose of an end effector of the robotic arm, determine, for each of the joints, a unified constraint of the joint based on a current position of the joint; and a joint motion from the current position that satisfies the unified constraint; and determine a trajectory for the joints based on respective joint motions for moving the end effector into the target pose.

[0008] In one embodiment, the unified constraint for each joint includes at least one of a position limit, a velocity limit, and an acceleration limit of the joint associated with the current position of the joint. In some embodiments, the instructions toAtorney Docket No. : AUR6374WOPCT2Electronically Filed determine the trajectory includes, responsive to a determination that the joint motion of a particular joint is to cause the particular joint to move to a joint position limit of the unified constraint, set a new position towards which the particular joint is to move to the joint position limit, and, for each remaining joint, set a respective new position towards which the joint is to move according to its respective joint motion. In another embodiment, the memory includes further instructions to determine a pose error between the target pose of the end effector and a starting pose of the end effector, where the joint motion of each joint is determined based on the pose error. In some embodiments, the memory includes further instructions to determine an intermediate pose of the end effector by scaling the pose error based on a magnitude of the pose error, where instructions to determine the joint motion includes instructions to: determine, for each j oint, a target j oint motion to cause the end effector to move towards the intermediate pose; and scale the target joint motion to ensure motion of the joint stays within one or more joint limits of the unified constraint.

[0009] In one embodiment, the memory includes further instructions to receive joint position data for the joints that indicate the current positions of the joints, where the instructions to determine the trajectory includes instructions to update, for each joint, respective joint position data according to the joint motion from the current position of the joint. In another embodiment, the memory includes further instructions to: determine a new pose of the end effector based on the updated joint position data; and output joint commands based on the updated joint position data to cause the end effector to move into the target pose responsive to the new pose converging with the target pose within a threshold. In another embodiment, the unified constraint is a first unified constraint and the joint motion is a first joint motion, where the memory includes further instructions to, responsive to the new pose failing to converge with the target pose within the threshold and until the new pose converges with the target pose within the threshold, for each of the joints, determine a second unified constraint of the joint based on a new position of the joint based on the updated joint position data, where the second unified constraint is different than the first unified constraint; determine a second joint motion from the new position that satisfies the second unified constraint; update respective joint position data according to the second joint motion; and update the new pose of the end effector based on the updated joint position data.Atorney Docket No. : AUR6374WOPCT2Electronically Filed

[0010] According to another embodiment of the disclosure, a method including: determining a current pose of a robotic arm that includes several joints; determining a target pose of the robotic arm; for each joint of the joints, determining a joint motion to move the joint from a current position to cause the current pose of the robotic arm to move towards the target pose; and establishing a unified constraint based on the current position of the joint, where the unified constraint includes one or more joint limits of the joint; scaling the joint motions of the joints based on established unified constraints; and provide joint commands to ensure synchronized motion between at least a portion of the joints based on the scaled joint motions.

[0011] In one embodiment, the method further including: determining a pose error based on a difference between the current pose and the target pose; determining a new pose of the robotic arm based on pose error, where the joint motions of the joints are determined based on the new pose of the robotic arm. In another embodiment, determining the new pose includes applying a gain to the pose error based on a magnitude of the pose error. In another embodiment, the method further including determining a resulting pose of the robotic arm according to the scaled joint motions of the joints, where the joint commands are provided responsive to the resulting pose converging with the target pose within a threshold. In another embodiment, the method further including iteratively, until the resulting pose of the robotic arm converges with the target pose within the threshold, determining, for each joint, a new joint position based on a previously determined resulting pose of the robotic arm; a new unified constraint based on the new joint position; and a new joint motion to move the joint from the new joint position while satisfying the new unified constraint; and determining a new resulting pose of the robotic arm based on the new joint motions.

[0012] In one embodiment, the unified constraint maintains the one or more joint limits that includes at least one of a joint position limit, a joint velocity limit, an acceleration limit, or a jerk limit. In another embodiment, the method claim further including: determining, for each joint, a scaling factor for a respective joint motion that prevents violation of any individual joint limit maintained by the unified constraint; and selecting a largest scaling factor of the determined scaling factors that ensures joint motion of the joints satisfy their respective unified constraint, where scaling the joint motions includes applying the largest scaling factor to each joint motion.Atorney Docket No. : AUR6374WOPCT2Electronically Filed

[0013] In one embodiment, the method further including: determining whether at least one scaled joint motion causes a respective joint to exceed at least one of a joint position limit, a joint velocity limit, or a joint acceleration limit based on the current position of respective joint; and in response, generating a respective joint command for the respective joint to move to or within a threshold of the joint position limit, the joint velocity limit, or the joint acceleration limit.

[0014] According to another embodiment of the disclosure, a system, an apparatus, or an electronic device as shown and as described herein. According to another embodiment of the disclosure, a method substantially as herein described. According to another embodiment of the disclosure, includes a processor configured to perform one or more operations as described herein. According to another embodiment of the disclosure, includes a non-transitory machine-readable medium that includes instructions which when executed by at least one processor causes a system to perform one or more operations as described herein.

[0015] The above summary does not include an exhaustive list of all embodiments of the disclosure. It is contemplated that the disclosure includes all systems and methods that can be practiced from all suitable combinations of the various embodiments summarized above, as well as those disclosed in the Detailed Description below and particularly pointed out in the claims. Such combinations may have particular advantages not specifically recited in the above summary.Atorney Docket No. : AUR6374WOPCT2Electronically FiledBRIEF DESCRIPTION OF THE DRAWINGS

[0016] The embodiments are illustrated by way of example and not by way of limitation in the figures of the accompanying drawings in which like references indicate similar elements. It should be noted that references to "an" or “one” embodiment of this disclosure are not necessarily to the same embodiment, and they mean at least one. Also, in the interest of conciseness and reducing the total number of figures, a given figure may be used to illustrate the features of more than one embodiment, and not all elements in the figure may be required for a given embodiment.

[0017] Fig. 1 shows a pictorial view of an example surgical system in an operating arena.

[0018] Fig. 2 shows an example of a user-side human interface device (HID) of the user console with which an operator may use to control a robotic component of the surgical system according to one embodiment of the disclosure.

[0019] Fig. 3 shows a patient-side robotic arm that includes a surgical tool of the surgical system according to one embodiment of the disclosure.

[0020] Fig. 4 is a block diagram of the surgical system for performing a constraint-aware inverse kinematics (IK) algorithm that maintains joint constraints while determining a trajectory for joint movement according to one embodiment.

[0021] Fig. 5 is a flowchart of one embodiment of a process for performing constraint-aware IK with trajectory planning for joint movement.

[0022] Fig. 6 is a flowchart of another embodiment of a process for performing constraint-aware IK with trajectory planning for joint movement.

[0023] Figs. 7A-7C illustrate examples of feasible joint velocities for trajectory planning.

[0024] Figs. 8A-8D illustrate other examples of feasible joint velocities with respect to different ranges of accelerations for trajectory planning.Atorney Docket No. : AUR6374WOPCT2Electronically FiledDETAILED DESCRIPTION

[0025] Several embodiments of the disclosure with reference to the appended drawings are now explained. Whenever the shapes, relative positions and other embodiments of the parts described in a given embodiment are not explicitly defined, the scope of the disclosure here is not limited only to the parts shown, which are meant merely for the purpose of illustration. Also, while numerous details are set forth, it is understood that some embodiments may be practiced without these details. In other instances, well-known circuits, structures, and techniques have not been shown in detail so as not to obscure the understanding of this description. Furthermore, unless the meaning is clearly to the contrary, all ranges set forth herein are deemed to be inclusive of each range’s endpoints.

[0026] Fig. 1 shows a pictorial view of an example (e.g., laparoscopic) surgical system (which hereafter may be referred to as “system”) 100 in an operating arena.The system 100 includes a user console 120, a control tower 131, and one or more surgical robotic arms 110 at a surgical robotic table (surgical table or surgical platform) 151. In one embodiment, the arms 110 may be mounted to a table or bed on which the patient rests as shown in the example of Fig. 1. In one embodiment, at least some of the arms 110 may be configured differently. For example, at least some of the arms may be mounted on a ceiling, sidewall, or in another suitable structural support, such as a cart separate from the table. The system 100 can incorporate any number of devices, tools, or accessories used to perform surgery on a patient 161. For example, the system 100 may include one or more surgical tools (instruments) 171 used to perform surgery (surgical procedure). A surgical tool 171 may be an end effector that is attached to a distal end of a surgical arm 110, for executing a surgical procedure.

[0027] Each surgical tool 171 may be manipulated manually, robotically, or both, during the surgery. For example, the surgical tool 171 may be a tool used to enter, view, or manipulate an internal anatomy of the patient 161. In an embodiment, the surgical tool 171 may include a grasper that can grasp tissue of the patient. In another embodiment, the surgical tool may include one or more cameras (e.g., an endoscopic camera), which may be configured to capture images of a surgical site in which one or more other surgical tools 171 may be used to perform one or moreAtorney Docket No. : AUR6374WOPCT2Electronically Filed surgical tasks. For instance, the camera may be arranged to have a field of view that includes a surgical site with one or more other surgical tools, which may be manipulated by an operator. The surgical tool 171 may be controlled manually by a bedside operator 180; or it may be controlled robotically, via actuated movement of the surgical robotic arm 110 to which it is attached. For example, when manually controlled an operator may (e.g., physically) hold a portion of the tool (e.g., a handle), and may manually control the tool by moving the handle and / or pressing one or more input controls (e.g., buttons) on the (e.g., handle of the) tool. In another embodiment, when controlled robotically, the surgical system may manipulate the surgical tool-based user input (e.g., received via the user console 120, as described herein).

[0028] Generally, a remote operator 190, such as a surgeon or other operator, may use the user console 120 to remotely manipulate the arms 110 and / or the attached surgical tools 171, e.g., during a teleoperation. The user console 120 may be located in the same operating room as the rest of the system 100, as shown in Fig. 1. In other environments however, the user console 120 may be located in an adjacent or nearby room, or it may be at a remote location, e.g., in a different building, city, or country. The user console 120 may include one or more components, such as a seat 119, one or more foot-operated controls (or foot pedals) 130, one or more human interface devices (HIDs) 140, and at least one display 150. In one embodiment, the user console may include less components. For example, the seat 119 may be separate from the user console. This may allow the user to sit on the seat and to move into a position that allows the user to use the console. In particular, the seat may be on casters that allow the seat to move on the floor.

[0029] The display 150 is configured to display, for example, a view of the surgical site inside the patient 161. The display may be configured to display image data (e.g., still images and / or video) that may be captured by a camera that may be used during a surgical procedure, as described herein. In one embodiment, the display may be any type of display, such as a liquid crystal display (LCD), a light-emitting diode (LED) display, an organic LED (OLED) display, etc. In some embodiments, the display may be a three-dimensional (3D) immersive display that is for displaying 3D (surgical) presentations. For instance, during a surgical procedure one or more endoscopic cameras may be capturing image data of a surgical site, which the displayAtorney Docket No. : AUR6374WOPCT2Electronically Filed presents to the user in 3D. The display may include a viewer that includes one or two screens that may be configured to display stereoscopic images. In this case, the viewer of the display may include a contoured portion that may be arranged to come into contact with and contour to at least a portion of the operator’s face when the remote operator 190 moves towards and / or comes into contact with the display. Coming into contact and contouring with the operator’s face may prevent ambient light from interfering with the displayed surgical presentation, while the screen(s) of the display 150 may give the remote operator 190 a perception of a 3D space captured by one or more cameras. In one embodiment, the 3D display may be an autostereoscopic display that provides 3D perception to the user without the need for special glasses. As another example, the 3D display may be a stereoscopic display that provides 3D perception with the use of glasses (e.g., via active shutter or polarized).

[0030] In another embodiment, the display 150 may be configured to display at least one graphical user interface (GUI) that may provide informative and / or interactive content, to thereby assist a user in performing a surgical procedure with one or more instruments in the surgical system 100. For example, some of the content displayed may include image data captured by one or more endoscopic cameras, as described herein. In another embodiment, the GUI may include selectable UI items, which when manipulated by the user may cause the system to perform one or more operations. For instance, the GUI may include a UI item as interactive content to switch control between robotic arms. In one embodiment, to interact with the GUI, the system may include input devices, such as a keyboard, a mouse, etc. In another embodiment, the user may interact with the GUI using the HID 140. For instance, the user may manipulate the HID to navigate through the GUI, (e.g., with a cursor), and to make a selection may hover the cursor over a UI item and manipulate the HID (e.g., selecting a control or button). In some embodiments, the display may be a touch-sensitive display screen. In this case, the user may perform a selection by navigating and selecting through touching the display. In some embodiments, any method may be used to navigate and / or select a UI item.

[0031] As shown, the remote operator 190 is sitting in the seat 119 and viewing the user display 150 while manipulating a foot-operated control 130 and a handheldAtorney Docket No. : AUR6374WOPCT2Electronically FiledHID 140 in order to remotely control one or more of the arms 110 and the surgical tools 171 (that are mounted on the distal ends of the arms 110.)

[0032] In some variations, the bedside operator 180 may also operate the system 100 in an “over the bed” mode, in which the bedside operator 180 (user) is now at a side of the patient 161 and is simultaneously manipulating a robotically-driven tool (end effector as attached to the arm 110), e.g., with a handheld HID 140 held in one hand, and a manual laparoscopic tool. For example, the bedside operator’s left hand may be manipulating the handheld HID to control a robotic component, while the bedside operator’s right hand may be manipulating a manual laparoscopic tool. Thus, in these variations, the bedside operator 180 may perform both robotic-assisted minimally invasive surgery and manual laparoscopic surgery on the patient 161.

[0033] During an example procedure (surgery), the patient 161 is prepped and draped in a sterile fashion to achieve anesthesia. Initial access to the surgical site may be performed manually while the arms of the system 100 are in a stowed configuration or withdrawn configuration (to facilitate access to the surgical site.) Once access is completed, initial positioning or preparation of the system 100 including its arms 110 may be performed. Next, the surgery proceeds with the remote operator 190 at the user console 120 utilizing the foot-operated controls 130 and the HIDs 140 to manipulate the various end effectors and perhaps an imaging system, to perform the surgery. Manual assistance may also be provided at the procedure bed or table, by sterile- gowned bedside personnel, e.g., the bedside operator 180 who may perform tasks such as retracting tissues, performing manual repositioning, and tool exchange upon one or more of the robotic arms 110. Non-sterile personnel may also be present to assist the remote operator 190 at the user console 120. When the procedure or surgery is completed, the system 100 and the user console 120 may be configured or set in a state to facilitate post-operative procedures such as cleaning or sterilization and healthcare record entry or printout via the user console 120.

[0034] In one embodiment, the remote operator 190 holds and moves the HID 140 to provide an input command to drive (move) one or more robotic arm actuators 170 (or driving mechanism) in the system 100 for teleoperation. The HID 140 may be communicatively coupled to the rest of the system 100, e.g., via a console computerAtorney Docket No. : AUR6374WOPCT2Electronically Filed system 160 (or host). The HID 140 can generate spatial state signals corresponding to movement of the HID 140, e.g., position and orientation of the handheld housing of the HID, and the spatial state signals may be input signals to control motions of the robotic arm actuators 170. The system 100 may use control signals derived from the spatial state signals, to control proportional motion of the actuators 170. In one embodiment, a console processor of the console computer system 160 receives the spatial state signals and generates the corresponding control signals. Based on these control signals, which control how the actuators 170 are energized to drive a segment or link of the arm 110, the movement of a corresponding surgical tool that is attached to the arm may mimic the movement of the HID 140. Similarly, interaction between the remote operator 190 and the HID 140 can generate, for example, a grip control signal that causes a jaw of a grasper of the surgical tool 171 to close and grip the tissue of patient 161.

[0035] The system 100 may include one or more user-side (or surgeon-side) HIDs 140, where respective control signals are generated for each HID that control the actuators and the surgical tool (end effector) of a respective arm 110. For example, the user console 120 may include two HIDs 140, a first (or left) HID arranged to be held and controlled by the operator’s left hand and a second (or right) HID arranged to be held and controlled by the operator’s right hand. In which case, the remote operator 190 may move the left HID 140 to control the motion of an actuator 170 that is in a one robotic arm, where the actuator responds by moving linkages, gears, etc., in that arm 110. Similarly, movement of the right HID 140 by the remote operator 190 controls the motion of another actuator 170, which in turn drives other linkages, gears, etc., of (e.g., another robotic component, such as a robotic arm of) the system 100. The system 100 may include a right arm 110 that is secured to the bed or table to the right side of the patient, and a left arm 110 that is at the left side of the patient. An actuator 170 may include one or more motors that are controlled so that they drive the rotation of a joint of the arm 110, to for example change, relative to the patient, an orientation of an endoscope or a grasper of the surgical tool 171 that is attached to that arm. Motion of several actuators 170 in the same arm 110 can be controlled by the spatial state signals generated from a particular HID 140. The HIDs 140 can also control motion of respective surgical tool graspers. For example, each HID 140 can generate a respectiveAtorney Docket No. : AUR6374WOPCT2Electronically Filed grip signal to control motion of an actuator, e.g., a linear actuator that opens or closes jaws of the grasper at a distal end of surgical tool 171 to grip tissue within patient 161.

[0036] In some embodiments, the communication between the surgical robotic table 151 and the user console 120 may be through a control tower 131, which may translate user commands that are received from the user console 120 (and more particularly from the console computer system 160) into robotic control commands that transmitted to the arms 110 on the surgical table 151. The control tower 131 may also transmit status and feedback from the surgical table 151 back to the user console 120. The communication connections between the surgical table 151, the user console 120, and the control tower 131 may be via wired (e.g., optical fiber) and / or wireless links, using any suitable one of a variety of wireless data communication protocols, such as BLUETOOTH protocol. Any wired connections may be optionally built into the floor and / or walls or ceiling of the operating room. The system 100 may provide video output to one or more displays, including displays within the operating room as well as remote displays that are accessible via the Internet or other networks. The video output or feed may also be encrypted to ensure privacy and all or portions of the video output may be saved to a server or electronic healthcare record system.

[0037] As described herein, this shows an example of the operator 190 using the user console 120 to control one or more robotic components, such as a robotic arm 110 and / or a surgical tool 171 coupled to a distal end of the robotic arm, of the surgical system 100. Specifically, this figure shows the operator 190 seated on the seat 119 in front of the user console 120, while controlling (or moving) one or more robotic components by manipulating control inputs, such as one or both of the HIDs 140 and / or the foot pedal(s) 130.

[0038] The user console 120 includes a base 201 to which the foot pedals 130 may be attached and a support structure 202 that may be mounted on the base 201 and extends vertically upward from the base. Coupled to the support structure 202 includes the display 150 and the HIDs 140. As shown, the HIDs may be coupled (mounted) on a bottom side of a structure (e.g., an arm rest) that is coupled to the support structure 202. In which case, the HID may be a wired or “grounded” input device that may be connected to the user console via a wired connection (e.g., to exchange data), which isAtorney Docket No. : AUR6374WOPCT2Electronically Filed in contrast to an “ungrounded’ input device that may be wirelessly connected to the user console (e.g., exchanging data via a wireless connection). As described herein, the HID 140 may include one or more joints that couple one or more links, which allow the HID to have one or more rotational degrees of freedom. In one embodiment, the HID may be grounded such that the joints of the HID may be arranged to hold their position and / or orientation absent to an external applied force upon the HID. More about the configuration of the HID is described herein.

[0039] In another embodiment, the HIDs may be coupled to the user console in other configurations. For example, the HIDs may be separate from the user console, but may be communicatively coupled (e.g., through wired-connection). In which case, the HIDs may be mounted on another device (e.g., a surgical cart), which may be coupled to the user console.

[0040] As described herein, the operator 190 may be using the user console to control a robotic arm during a teleoperation. As shown, the user may be viewing a surgical workspace through the display 150, while operating one or more components of the surgical system by manipulating one or both of the HIDs 140 and / or the foot pedals 130.

[0041] Fig. 2 shows an example of the HID 140 of the user console 120 with which the operator 190 may control a robotic arm of the surgical system 100 during a surgical procedure. In particular, this figure shows the HID 140 that includes a support arm 200 and a gimbal 205, which may include one or more links 210 coupled together by one or more joints 215. As described herein, the HID 140 may be a low-inertia such that as the user manipulates the HID, one or more joints may move (or be adjusted) such that the HID may be moved between positions (or poses) within a workspace surrounding the user console 120. The support arm 200 may be configured to couple the gimbal 205 to the user console 120, where the gimbal may be arranged to rotate about one or more axes and / or the support arm may be arranged to rotate about one or more axes to allow the user to move an end user control 220 of the gimbal within three- dimensional (3D) space. Movement of the end user control may allow the user to control (e.g., movement of) a robotic component, such as a robotic arm, of the surgicalAtorney Docket No. : AUR6374WOPCT2Electronically Filed system. In one embodiment, each of the joints may be capable of rotating about and / or translating along one or more axes.

[0042] As shown, the HID 140 includes seven joints 215a-215g, which may provide the HID 140 with seven (rotational) degrees of freedom (DoF). In particular, the support arm 200 includes three links 210a-210c and three joints 215a-215c. In particular, link 210a may be coupled to link 210b, via joint 215b, and link 210b may be coupled to link 210c, via joint 215c. In one embodiment, the HID 140 may be arranged to couple to the support arm 200 at the joint 215a. The gimbal 205 may be coupled to the support arm 200 via joint 215d. The gimbal includes three links 210d-210f and four joints 215d-215g. Link 210d may be coupled to joint 215d and joint 215e. Link 210d may be coupled to link 210e, via joint 215e, and link 210e may be coupled to link 210f, via joint 215f. The gimbal 205 may also include an end user control 220 that may be coupled to link 21 Of via joint 215g.

[0043] As described herein, the HID may include seven DoFs. In one embodiment, one or more DoF may be redundant with respect to other DoF. For example joint 215d may be redundant with respect to joint 215f, since both joints allow the end user control 220 to rotate about a same axis (e.g., a Z-axis). In which case, the system 100 may be configured to provide null space motion in which one or more joints of the HID may be moved, while a pose of an end effector (e.g., the end user control 220) may remain unaffected. As described herein, this redundancy may allow the system to perform one or more secondary tasks by manipulating one or more joints, without affecting the end user control 220. For instance, as described herein, the gimbal 205 may include four joints 215d, 215e, 215f, and 215g, where the last three may provide a 3D range of motion, and the first may be redundant by having a redundant range of motion as joint 215f, whereby motion of joint 215d may occur without affecting (or causing motion) to the other three joints. In one embodiment, a secondary task may include an adjustment of one or more redundant joints in order to provide the user with a more ergonomic position of the end user control 220, without affecting the pose of the end user control.

[0044] In one embodiment, the HID 140 may include more or fewer links and / or joints, which may provide more or fewer DoFs for the HID. For example jointAtorney Docket No. : AUR6374WOPCT2Electronically Filed215a may be arranged to rotate the entire HID about an axis (e.g., the Z-axis) with respect to the link 210a. Conversely, joint 215b may be arranged to rotate link 210b (and the links / joints to which the link 210b may be coupled) about an orthogonal axis (e.g., Y-axis) to the axis that runs through link 210a and joint 215b.

[0045] In one embodiment, at least some of the joints may include one or more motors (or actuators) that may allow the joint to rotate around and / or translate along one or more axes. In this way, the HID may provide a user with assisted movement in response to user input, such as a user applying an external force upon the end user control 220. In another embodiment, the HID 140 may be arranged to provide haptic feedback to the user. As described herein, the HID may be configured to control movement of a robotic component, such as a surgical tool coupled to a distal end of a robotic arm. As a result of this movement, external forces may be applied onto the surgical tool when the tool presses up against an object, such as pressing up against an abdominal wall of a patient during a surgical procedure. This force applied by the object may be haptically applied by the HID such that the operator may perceive the force. In one embodiment, the system 100 may be configured to determine whether an external force is being applied to the robotic arm, such as through one or more sensors (e.g., force sensors). In another embodiment, the system may determine that the robotic arm is pressing onto an object, based on a comparison between control commands from the HID and encoder data from one or more encoders of the robotic arm. Upon determining this external force, the system may be configured to provide haptic feedback to the user by controlling one or more motors of the HID to relay an applied force through the HID onto the user. In one embodiment, this applied force may be an opposite (and / or proportional) external force applied onto the robotic arm. More about providing haptic feedback force is described herein.

[0046] Fig. 3 shows a patient-side robotic arm 110 that includes the surgical tool 171 of the surgical system 100 according to one embodiment of the disclosure. The surgical arm includes ten links 303a-303j that are coupled together through nine joints 302a-302i, where each joint may be arranged to move a correspondingly coupled link by at least one of rotating the link about at least one axis and / or translating the link along at least one axis. For instance, each joint may include an actuator, which may move its corresponding joint based on user input (e.g., user commands) receivedAtorney Docket No. : AUR6374WOPCT2Electronically Filed through movement of the HID 140. In one embodiment, the robotic arm may include more or less joints and / or links.

[0047] The robotic arm may be coupled to a structure (such as the surgical table 151 or a surgical cart) at the most proximal joint 302a. Coupled to the most distal link 303j includes a tool drive 305 configured to control a surgical tool 171. In another embodiment, any type of end effector may be coupled to the tool drive, such as an endoscopic camera. Coupled to the tool drive 305 is a cannula 304 that may be inserted into a patient’s cavity (e.g., abdominal region), where the (e.g., end effector of the) surgical tool may be received through the cannula.

[0048] As described herein, the system 100 may be configured to control movement of the robotic arm based on user input through the HID 140. In particular, the system may be configured to cause one or more joints of the robotic arm to move in order to match motion of the HID. To match motion, the system may attempt to align (or match) a pose of the (e.g., end user control 220 of the) HID 140 with that of the surgical tool 171 of the robotic arm with respect to at least one reference frame. For instance, the system 100 may be configured to receive user input through the HID, where the user input may indicate one or more spatial state signals corresponding to movement of the HID, as described herein. From this input, the system may determine a pose of the HID, or more specifically a pose of the end user control of the HID, and may be configured to generate one or more control (or joint) commands to cause one or more joints of the robotic arm to actuate in order for a pose of the surgical tool coupled to the robotic arm to match the pose of the end user control, as perceived by the user through the display 150 of the user console 120. The system may perform one or more conversions (or transformations) of the HID pose with respect to one or more reference frames into the pose of the tool 171 with respect to the reference frames. One reference frame may be with respect to the display. As described herein, the user may control the surgical instrument by manipulating the HID, while viewing the surgical instrument through the display 150 that may receive video images from one or more cameras. In which case, the system may define tool motion with respect to the camera to follow the HID motion with respect to the display. Thus, the system may convert a user command (e.g., HID pose based on position data from sensors, such as encoders of the HID) that may with respect to the display 150 to a target (or desired) tool pose with respect to aAtorney Docket No. : AUR6374WOPCT2Electronically Filed camera of the system 100. As a result, the system may ensure that the pose of the HID matches the pose of the surgical tool, where both may share a global reference frame. In one embodiment, the system may be configured to adjust a pose of the surgical tool according to movement (or changing poses of the HID) in real-time, whereby motion of the tool may track motion of the HID in real-time and during a surgical procedure.

[0049] To control surgical instruments, robotic systems employ an inverse kinematics (IK) process to calculate joint parameters of a robotic arm that are needed to move surgical instruments. Minimally invasive surgical procedures demand highly precise and smooth robotic control to navigate delicate anatomical structures, but conventional IK algorithms fail to satisfy these requirements. These IK algorithms face challenges in handling joint-level constraints, such as joint position limits, joint velocity limits joint acceleration limits, and jerk limits, which are critical for avoiding mechanical failure of the robotic joints and maintaining accuracy of surgical instrument movements during a surgical procedure. For instance, a challenge with conventional IK includes generating joint velocities that drive a robotic arm toward a target pose while respecting the arms’ physical limits or constraints. In an MIS utilizing surgical robotic arms, these challenges may be particularly apparent as smooth and predictable motion may be desirable to reduce the possibility of tissue damage, to enable precision, and to provide intuitive control for surgeons. Existing IK methods do not account for motion smoothness as part of the IK algorithm, thereby leading to jerky or abrupt movements that may compromise the desired instrument pose. In addition, these algorithms lack adequate robustness that may result in a reduced success rate in finding feasible instrument positioning for complex robotic arm configurations and in highly constrained workspaces, such as those encountered in minimally invasive surgery (e.g., inside a patient’s abdomen). Therefore, there is a need for an IK algorithm that accounts for joint constraints to produce an efficient and robust trajectory for one or more joints for surgical instrument movement.

[0050] The present disclosure provides a method and system for performing a constraint-aware IK algorithm that maintains joint constraints for determining a trajectory of joint movement for a robotic arm to move a surgical instrument into a target (or desired) pose. The system 100 receives, via the HID 140, a target pose of an end effector of the robotic arm 110. The target pose may corresponding to a HIDAtorney Docket No. : AUR6374WOPCT2Electronically Filed command that may include a HID pose based on user movement of the end user control 220. The system determines a trajectory for moving one or more joints of the robotic arm 110 to cause the end effector 171 to move to the target pose. The trajectory may include a series of joint movements, each joint movement indicating a joint motion, such as a joint velocity, from a position of a joint that satisfies a unified constraint of the joint associated with the position of the joint. The unified constraint may be determined for each joint, where the unified constraint may indicate feasible joint motion that satisfies (or does not violate) one or more joint limits, such as a position limit, a velocity limit, an acceleration limit, and / or a jerk limit of the joint. As described herein, the incorporation of differential constraints may improve motion smoothness. In particular, the system may perform an iterative process in which the system may generate the trajectory by determining joint movements for moving the end effector towards the target pose. For each joint movement from a current joint position, the system may determine an associated unified constraint, which may be used by the system to determine the unified joint motion across multiple joints. The system 100 may output the trajectory to cause the robotic arm to move the end effector into the target pose. As a result, the system of the present disclosure integrates joint-level constraints directly within the IK algorithm to determine joint motion (e.g., joint angles and / or positions) required to achieve a target instrument pose (e.g., position and orientation) in space, as commanded by the operator, such that the solution satisfies the joint level constraints while giving robust solutions in complex robotic arm configurations and in highly constrained workspaces. By iteratively determining joint motion along the trajectory while ensuring that joint limits are bounded, the system may ensure smooth motion from a starting position to the target position. Moreover, to determine joint positions, the present disclosure provides a robust optimization framework, which overcomes the limitations of the conventional IK methods to provide these robust solutions when constraints are considered.

[0051] Fig. 4 is a block diagram of the surgical system 100 for performing an IK algorithm that maintains joint constraints while using a trajectory for joint movement to achieve a target pose of the surgical tool 171 according to one embodiment. The system includes the HID 140, the robotic arm 110, and a controller 400. In one embodiment, the system may include more or less components, such asAtorney Docket No. : AUR6374WOPCT2Electronically Filed having two or more robotic arms, each with one or more tools 171 that may be arranged to be manipulated by one or more HIDs.

[0052] The robotic arm 110 includes one or more sensors 460, one or more actuators 170, and one or more surgical tools 171. As described herein, the actuators may be a part of and / or cause one or more joints of the arm to move based on joint commands. This movement may cause corresponding joints to rotate and / or translate about one or more axes. The sensors may be configured to produce position data of the robotic arm 110. For instance, the sensors may include encoders, each of which may be configured to measure a joint position (e.g., translational and / or rotational values) as encoder (or position) data of a respective joint. In which case, the position data from the sensors 460 may indicate the actual position of a respective joint in space (with respect to a reference point or frame). As described herein, this position data may be used to determine an actual tool pose of the tool 171 coupled to the robotic arm 110.

[0053] In one embodiment, the controller 400 may be a special-purpose processor such as an application-specific integrated circuit (ASIC), a field- programmable gate array (FPGA), a general-purpose microprocessor, a digital signal controller, or a set of hardware logic structures (e.g., arithmetic logic units, filters, and dedicated state machines). In one embodiment, the controller 400 may be a part an electronic device, such as the console computer system 160, the control tower 131, and / or the user console 120. Although illustrated as being a single component, the controller may include one or more electronic components (e.g., processors, memory, etc.) that may be communicatively coupled on a single electronic device (such as the console computer system 160), or across multiple devices (e.g., communicating over a wireless computer network). In some embodiments, the controller 400 may be a part of a separate device, such as a part of a remote server that may be in communication with one or more electronic devices of the surgical system 100. In which case, the remote sever may be configured to communicate between the user console for receiving user commands and the control tower 131 for transmitting control signals (e.g., joint commands) for moving the arms 110.

[0054] The controller 400 may be configured to perform constraint-aware IK operations for generating a trajectory for controlling one or more actuators 170 of theAtorney Docket No. : AUR6374WOPCT2Electronically Filed robotic arm to move one or more joints in order to position the tool 171 into the target pose. The controller 400 includes an IK 420 and a HID / Tool pose estimator 410. The controller 400 may be configured to receive a user command from the HID 140, responsive to user input, such as the user moving and manipulating the end user control 220. The user command may include position data (e.g., spatial state signals) from the HID 140, responsive to the user input. The position data may be produced by one or more sensors of the HID, such as encoders, which may be arranged to measure joint positions as the user moves the end user control. The HID / Tool pose estimator 410 may be configured to estimate a pose of the HID based on the user command from the HID. The HID pose may be a six degrees of freedom (6 DoF) pose of the end user control 220 (e.g., in three-dimensional (3D) Cartesian space) that is being held by the user while moving the HID. The estimator 410 may be configured to convert the user command into the HID pose. For instance, when the user command includes position data of one or more joints of the HID, the estimator 410 may use a forward kinematics algorithm to convert the data into the HID pose.

[0055] The HID / tool pose estimator 410 may be configured to determine a target pose of the (e.g., surgical tool 171, which may be coupled to the) robotic arm 110, which may be referred to hereafter as “target tool pose”. The estimator 410 may be configured to convert the HID pose into the target tool pose. The conversion may allow for the pose of the tool and the HID pose to be the same with respect to one or more reference frames. As described herein, the user may control the surgical instrument by manipulating the HID, while viewing the surgical instrument through the display 150 that may receive video images from one or more cameras. In which case, the system may convert the HID pose into the target tool pose such that tool motion with respect to the camera follows the HID motion with respect to the display. Thus, the system may convert a user command (or HID pose based on position data from the HID) that may with respect to the display 150 to a target tool pose with respect to a camera of the system 100. The HID / tool pose estimator 410 may be configured to determine the actual (current) tool pose of the (tool 171 of the) robotic arm 110 based on sensor data from the sensors 460. For instance, the estimator 410 may perform a forward kinematics process to convert the sensor data into a 6 DoF tool pose of the tool 171.Atorney Docket No. : AUR6374WOPCT2Electronically Filed

[0056] In one embodiment, the operations of the estimator 410 may be performed on a different device. For instance, when the controller 400 is a part of an electronic device within the surgical operating room, such as the control tower 131, the desired tool pose may be received (as a user command) from a separate electronic device, such as the console computer system 160. In which case, the console computer system 160 may perform at least some of the operations of the estimator 410 to produce a target tool pose responsive to user input through the HID, and may provide the target tool pose as a user command (e.g., through a computer network) to the controller 400.

[0057] The IK 420 may be configured to perform an iterative process for generating a trajectory for one or more joint movements of the robotic arm 110 to cause the tool 171 to move into the target tool pose, while maintaining (or satisfying) one or more constraints of the robotic arm. To perform at least some of these operations, for each iteration, the IK 420 includes a tool pose error estimator 425, a joint motion estimator 430, a unified constraint estimator 440, and a trajectory planner 450.

[0058] The tool pose error estimator may be configured determine a tool pose error based on a difference between the current tool pose and the target tool pose. In one embodiment, the current tool pose may be based on current position data from one or more sensors 460 of the robotic arm 110, such as performing a forward kinematics conversion of the position data into the tool pose. The tool pose error may indicate a deviation or estimate of how different the actual tool’s position and / or orientation are from the target tool position and / or orientation. In one embodiment, the tool pose error may represent a path (e.g., straight-line pattern) along which the tool may traverse in order to reach the target tool pose from the tool’s current pose. In particular, the tool pose error may represent an estimate of the movement through which the tool 171 is to move from its current position to the target position.

[0059] For the tool 171 to traverse the tool pose error in order to reach the target tool pose may require a considerable amount of movement by the (e.g., robotic arm 110 of the) tool 171. Computing the joint movements of the robotic arm based on a large tool pose error (e.g., above a threshold) may cause jerking motions in the robotic arm. Therefore, the tool pose error estimator 425 may be configured to perform pose error handling operations to determine a series of smaller joint motions for moving theAtorney Docket No. : AUR6374WOPCT2Electronically Filed tool towards its target position. As described herein, the pose error may be defined as a path along which the tool may traverse. From this path, the estimator 425 may be configured to derive portions of the path along which the tool may be driven. To do this, the estimator 425 may be configured to determine one or more intermediate poses of the tool 171 between a starting pose of the tool 171 (e.g., the pose of the tool from before user input is received) and the target tool pose, whereby the IK algorithm may iteratively compute joint motions for the tool to converge towards the target tool pose. By iteratively computing smaller motions, the system may be configured to enable smooth motion between the current (original) pose to the target pose of the tool. In which case, the tool pose error estimator 425 may be configured to determine an intermediate pose of the tool to account for at least a portion of the tool pose error.

[0060] The estimator 425 may determine an intermediate pose based on one or more limits of the robotic arm. For instance, the estimator 425 may determine the pose by taking into account at least one of a velocity limit, a position limit, and / or an acceleration limit of the robotic arm. In one embodiment, the limits may be predefined limits based on the physical configuration of the robotic arm. To determine the intermediate pose, the estimator 425 may be configured to perform one or more optimization operations to find the intermediate pose that satisfies an objective function to maximize movement of the surgical tool 171, while not exceeding the arms limits (e.g., staying equal to or below the limits). These operations may include a neighborhood line search in which a pose of the surgical tool 171 is determined in which the objective function converges to a local minimum.

[0061] In another embodiment, the estimator 425 may use other methods to determine the intermediate pose, such as gain scheduling that may be based on the determined pose error. For instance, the estimator 425 may be configured to apply a e.g., non-linear) scaling factor (or gain) to the pose error based on the magnitude of the pose error. For example, when the pose error is greater than a first threshold, the estimator may apply a first scaling gain to reduce the pose error, where the intermediate pose may be based on the reduced pose error. Conversely, when the pose error is less than a second threshold, the estimator may apply a second scaling gain to increase the pose error. Scaling the error may enable the system to reduce the overall convergence time between the current pose to the target tool pose. In addition, scaling the error mayAtorney Docket No. : AUR6374WOPCT2Electronically Filed ensure that motion remains smooth by not causing the system to move the arm over great distances. Thus, for large pose errors, low gains may ensure smooth motion, whereas when the pose error is small, high gains may ensure that the tool converges within a reasonable amount of time. As described herein, the system 100 may perform an iterative process in which a trajectory is determined along the pose error. Thus, as the trajectory is determined, the pose error may shrink. As a result, the estimator 425 may be configured to adjust the scaling factor based on the remaining pose error. In another embodiment, the estimator 425 may implement error clamping, whereby the estimator 425 may define one or more intermediate poses between maximum amounts of pose within the pose error. As a result, the system 100 may improve the IK’s success rate by reducing the area of the tool movement for deriving corresponding joint movements, as described herein. Moreover, the IK utilizes this advanced gain scheduling to adaptively modify gains, thereby increasing the success rate of achieving (converging to) a target tool pose in a constrained environment, while improving motion smoothness using a closed-loop algorithm.

[0062] The joint motion estimator 430 may be configured to determine joint motion of (e.g., one or more actuators 170 of the) one or more joints of the robotic arm 110 in order to move the tool 171 into a new pose based on the estimated tool pose error. This new pose may correspond to the intermediate pose estimated by the pose error estimator 425, or may correspond to the target tool pose. The joint motion may include atarget joint velocity, atarget joint position, and / or a target joint acceleration for one or more j oints of the robotic arm that may be required for moving the tool 171 from its current position to a new pose. In which case, the joint motion estimator 430 may be configured to receive joint position data indicating the current joint positions of one or more joints of the robotic arm 110 and may receive a target pose (e.g., an intermediate pose) for the tool 171, and may be configured to generate one or more target joint motions (e.g., positions, velocities, and / or accelerations) based on the received data. The joint motion estimator 430 may be configured to perform a numerical optimization method as an iterative process to find the target joint motions that may result in a pose of the tool that may converge with the target pose of the tool. For instance, the joint motion estimator 430 may employ damped least-squares to identify joint motion towards the target pose. In one embodiment, this method mayAtorney Docket No. : AUR6374WOPCT2Electronically Filed address robotic singularities by dynamically varying a damping factor to maintain robustness near singular configurations. In another embodiment, the joint motion estimator 430 may perform another optimization method by performing one or more iterations, such as Newton Steps, in order to derive joint motion for moving the tool into the target tool pose.

[0063] As described thus far, the tool pose error estimator 425 may be configured to determine one or more intermediate poses of the surgical tool 171 for causing the tool to traverse into the target tool pose, where the intermediate poses may be based on a portion of the tool pose error. The joint motion estimator 430 may be configured to determine joint motion in order to move the tool 171 from its current pose to the target pose, through each intermediate pose. This estimation may account for limits (or constraints) of the surgical tool (e.g., in Cartesian space). The IK 420 may also be configured to estimate joint motion that takes into account other constraints, such as joint limits. The IK 420 may determine feasible joint motion of one or more joints that satisfy (or maintain) joint constraints, such as a joint position limit, a joint velocity limit, and / or a joint acceleration limit, which may be predefined limits, as described herein. Feasible joint motion for each joint may be based on the current position of the respective joint. For example, joint motion may be based on the joint position limit, which may range from Qmin that may represent a minimum rotational or linear travel allowed for the joint and Qm x that may represent the maximum travel allowed for the joint. When the current joint position is far away from a joint limit, such as being beyond a threshold from Qmin and / or Qmax, the joint may be capable of moving at a high feasible velocity based on the amount of time of the movement. The joint may be capable of moving at a high velocity since fast joint motion may not cause the joint to reach one of the joint limits. If, however, the current joint position were to be close to its joint limit, such being within the threshold described herein, then the feasible joint velocity may be reduced (e.g., lower than the previous example), since the joint is only capable of moving a short distance for the same amount of time. Thus, a joint’s feasible velocity may be dependent upon its position within its range of motion.

[0064] As described herein, the robotic arm 110 may include physical constraints, which may dictate motor acceleration. Constraints, however, may be difficult to formulate. For example, when the motor is near a positive position limit, theAtorney Docket No. : AUR6374WOPCT2Electronically Filed motor should decelerate to avoid going over the position limit. If the motor is near a negative position limit, then the motor should accelerate to avoid going below the negative position limit. When a motor is “far” from the positive and negative position limit (e.g., beyond one or more thresholds from both limits), then the motor may accelerate or decelerate, creating a sign ambiguity. The motor may be constrained by other physical limits. For instance, if the motor is accelerating during a timestep, the motor may not be able to decelerate a lot in the next timestep because the physical system may not be able to suddenly change force direction. As a result, it may be difficult to formulate bounds for acceleration, position, velocity, and jerk limits. Therefore, to address constraints, the IK 420 may be configured to compute a unified constraint at each joint position to define the upper and lower velocity bounds for the joint position, where the unified constraint can account for limits, such as position, velocity, acceleration and / or jerk together with respect to each other. As described herein, the system may define the unified constraint to synchronously scale joint motion in order to provide more smooth motion. This improved motion smoothness may be characterized by a reduction in a jerk profile of the generated joint motion and bounded first and second order (e.g., velocity and acceleration) derivatives of the joint motion.

[0065] The unified constraint estimator 440 may be configured to determine a unified constraint that may take into account one or more constraints of one or more joints of the robotic arm 110, which may be used by the IK 420 to determine the feasible motion of each joint. For example, a joint being controlled by the system 100 may have physical limitations that ensure correct operation. These limitations may include position limits, velocity limits, acceleration limits, jerk restrictions, etc. The constraint estimator 440 may be configured to produce a unified constraint based on these limitations. Some constraints to be unified may include:Position Limit: Qmin< q < QmaxVelocity Limit: Vmin< q < VmaxAcceleration Limit: Amin< q < AmaxAtorney Docket No. : AUR6374WOPCT2Electronically Filed

[0066] where q is a possible position of the joint within a position range of Qmin tO Qmax^ which may be measured in radians, q, which is a first derivative of q, is a possible velocity of the joint within a velocity range of Vmin to Vmax, which may be measured in radians / second (rad / s), and q, which may be a second derivative of q, is a possible acceleration of the joint within an acceleration range of Amin tO A max. which may be measured in rad / s2. Each of the constraints may represent physical limits of the robotic arm, such as the actuators 170 and / or the joints of the arm. Constraints may be determined, for example, based on a specification, data sheet, or stored value in a data structure (stored in memory of the controller 400). As described herein, the constraints may provide one or more boundaries for the trajectory planner 450 to determine joint motion. As described herein, the unified constraint may be determined based on other limits, such as a range of predefined jerk limits.

[0067] The unified constraint estimator 440 may be configured to determine a unified constraint as a range or boundary,which represent the minimum and maximum limits of the unified constraint that combines one or more of the limits described herein. The unified constraint may be as follows, where for i =

[0068] where Qmin.i may be negative. In the unified constraints, “i ” represents a joint of the robotic arm, such as where i=l, may correspond to a first actuator arranged to drive joint 302a of the robotic arm 110, “qk ” may represent a current position of the joint in radians, which may be measured from a corresponding encoder or sensor of the joint. “7” may represent a time step (e.g., in seconds). Thus, both Qmin.t and Qmax.t indicate three velocities, the first velocity limit with respect to position, the second velocity limit with respect to velocity, and a third velocity limit with respect to acceleration, where Qmin.i identifies the maximum negative velocity, and Qmax.i identifies the minimum positive velocity, thereby indicating a feasible velocity range for a corresponding joint. As a result, all of the limits may be combined in this unifiedAtorney Docket No. : AUR6374WOPCT2Electronically Filed constraint range so that the IK 420 may apply a single “limit” per joint to determine a feasible joint motion command (e.g., a feasible joint velocity).

[0069] The trajectory planner 450 may be configured to determine a trajectory for moving one or more joints, where the joint movements is to satisfy feasible joint motion associated with a position (e.g., the position at which the joint movement is to occur) while the unified constraint of the joint is maintained. In particular, the trajectory planner 450 may be configured to determine a trajectory for each joint based on the target joint motion (e.g., target joint velocity) derived by the joint motion estimator 430 according to the unified constraint s). In one embodiment, the trajectory may include one or more joint movements, such as one or more velocities, for moving a joint (with respect to time). In another embodiment, the trajectory may indicate joint positions of one or more joints of the robotic arm 110 with respect to time. Thus, the planner may be configured to determine feasible joint motion (e.g., feasible velocity) of each joint at each joint position that satisfies the unified constraint, and may be configured to scale a target joint motion forthat joint. In one embodiment, the planner may be configured to maximize the possible joint motion by solving an optimization problem that may be simultaneously constrained by the unified constraints. The problem may include: maximize aSubject to

[0070] The trajectory planner 450 may be configured to solve the optimization problem, such as a Quadratic Programming (QP) optimization problem, by determining a scaling factor, a, to scale q representing the target joint motion of the joint such that the target joint motion is within the lower bound and the upper bound of the unified constraint. The trajectory planner 450 may perform this determination for each joint, thereby identifying a scaling factor that ensures that target joint motion satisfies the joints respective unified constraint. The trajectory planner may then identify a maximum scaling factor such that when applied to a joint motion will satisfy the corresponding joint’s unified constraint. In other words, the planner may maximize the scaling factor, according to the bounds of at least some unified constraints at a time, &, to estimate each joint’s motion for its given position. Thus, the estimator 440 mayAtorney Docket No. : AUR6374WOPCT2Electronically Filed determine the feasible joint motion of all joints, determine respective scaling factors based on the joint motion and feasible joint motion, and then apply a scaling factor across at least some joint motions that ensures the resulting joint motions satisfy the unified constraints (e.g., of all moving joints) in order to provide synchronized motion across the joints. As a result, the system 100 may be configured to produce a smooth synchronized motion across the joints by applying gain scheduling operations to iteratively step through the pose error and by bounding joint limits through each step to ensure that joint motion maintains or satisfies unified constraints.

[0071] Figs. 5 and 6 are flowcharts of processes 500 and 600, respectively, for performing constraint-aware IK with trajectory planning. At least some of the operations of these processes may be performed by the controller 400 of the system 100. In another embodiment, at least some of the operations may be performed by another electronic device, which may communicate with the controller 400. Some of these processes may be performed in “real-time”, during a surgical procedure in which an operator is controlling a robotic arm to perform one or more surgical tasks.

[0072] Turning to Fig. 5, this figure illustrates a flowchart of one embodiment of the process 500 for performing constraint-aware IK with trajectory planning for joint movement. The process 500 begins by the controller 400 receiving a target tool pose of the surgical tool of the robotic arm (at block 505). The target tool pose may be received based on user input through the HID 140. The target tool pose may be based on a user command for adjusting a position and / or orientation of the tool 171 based on HID motion. The controller 400 may receive joint position data for at least one of the joints of the robotic arm (at block 510). This position data may be received from the sensors 460 of the robotic arm, as described herein. The system may determine a current tool pose of the surgical tool based on the position data (at block 515). The system may perform a forward kinematics function to determine the current tool pose, which may be the pose from which the tool is to move into the target tool pose.

[0073] The system 100 determines a tool pose error based on the target tool pose and the current tool pose (at block 520). As described herein, the estimator 425 may be configured to determine the pose error as a path along which the tool may traverse in order to reach the target tool pose. The system 100 determines anAtorney Docket No. : AUR6374WOPCT2Electronically Filed intermediate tool pose based on the tool pose error (at block 521). As described herein, the tool pose error estimator 425 may be configured to determine a pose error, and from that error may determine an intermediate tool pose based on a portion of that determined pose error. During a first iteration of the process 500, the intermediate pose of the end effector may be between the current (or starting) pose of the tool 171 and the target tool pose. This intermediate tool pose may be determined by the tool pose error estimator 425, and based on an error between the current pose and the target tool pose. The intermediate pose may be determined based on one or more limits of the robotic arm, as described herein. In another embodiment, the intermediate tool pose may be determined by determining the pose error and scaling the pose error based on the magnitude of the error. In one embodiment, the magnitude (or size) may be the difference (e.g., a length) between the target pose and the current pose. As described herein, the estimator 425 may apply a (e.g., non-linear) scaling gain to the tool pose based on the magnitude of the pose error. For instance, when the pose error is large (e.g., a size of the pose error being greater than a threshold), the estimator 425 may apply a small gain, whereas when the pose error is small (e.g., the size being less than the threshold), the estimator may apply a large gain (greater than the small gain). In one embodiment, a non-linear gain may ensure that the system prioritizes smooth motion when the pose error magnitude is large, and may ensure that the system priorities joint position when the pose error magnitude is small. In other words, without the scaling gain, when the pose error is small, it may take a considerable amount of time to converge the tool pose. With the scaling gain, however, the system may converge faster.

[0074] When this process 500 is performed through multiple iterations, the error estimator 425 may determine multiple intermediate poses in order for the tool pose to converge with the target tool pose. The pose error estimator may determine a same intermediate tool pose for each iteration. For example, the system may determine an intermediate pose for each iteration as being a percentage of the total tool pose error. In another embodiment, the pose error estimator 425 may determine a different intermediate tool pose for each iteration. This difference may be based on changing robotic arm limits as the tool pose changes and / or changes to the overall tool poseAtorney Docket No. : AUR6374WOPCT2Electronically Filed error, thereby resulting in different scaling gains to be applied to the error in order to derive the intermediate pose.

[0075] The system 100 determines a target joint motion of each joint of the robotic arm for moving the surgical tool into (or towards) the intermediate tool pose (at block 525). In particular, the joint motion estimator 430 may be configured to determine joint motion of one or more joints for moving the robotic arm to cause the tool to traverse between poses. For instance, the joint motion estimator 430 may determine target velocities of the joints from their current joint positions that would result in the robotic arm moving from its current pose into the intermediate pose. These target joint velocities may be determined while taking into considerations of one or more limits of the tool 171, which may be predefined limits. As described herein, the target joint motions may be determined by performing an optimization method for deriving joint motion solutions that results in tool motion into the intermediate tool pose.

[0076] The system 100 determines, for each joint, a unified constraint of the joint based on a current position of the joint (at block 530). The unified constraint estimator 440 may determine the unified constraint by determining one or more limits of the joint based on the joint’s current position. As described herein, limits of joints may be based on their current positions with respect to their limits. Thus, a joint at a position may be associated with one unified constraint, but may have a different unified constraint when the joint is at a different position.

[0077] The system determines whether a joint will exceed a joint position limit based on the target joint motion (at decision block 540). For instance, the target joint motion derived by the joint motion estimator may represent motion of joints in order for the robotic arm 110 to move into the target pose. This motion, however, may not account for one or more joint limits, such as position limits. In which case, the system 100 may be configured to determine whether the target motion may result in a joint reaching or exceeding its range of motion. For instance, the system may apply a joint target motion over a given time period (e.g., one or more clock cycles) to determine how far the joint may move. The system may determine whether the resulting joint position due to the movement may exceed or come within a threshold distance of aAtorney Docket No. : AUR6374WOPCT2Electronically Filed joint position limit, if the joint were to move at the target velocity for the given time period. This determination may be based on the scaling factor that may be applied by the trajectory planner. As described herein, the planner may scale each of the target joint motions using a scaling factor that satisfies the feasible velocities of each joint’s unified constraint. If, however, a joint is near at least one of its limits, such as its position limit, its scaling factor may be approximately zero and therefore there would not be joint motion of any of the joints. To avoid this, the system 100 determines whether any joint may exceed its position limit. If so, the system may set each of the joint’s new position according to its unified constraint (at block 545). In particular, for a joint that is to exceed its joint position limit (or within a threshold), the system may set the joint’s new position to a position at its respective joint limit or before the limit, such as at the threshold. In which case, upon determining that a joint motion is to reach or exceed its joint limit, the system may adjust that motion to cause the joint to stop at its respective joint position limit (or within the threshold) while still satisfying its respective unified constraint. For the remaining joints, the system may set each joint’s new position according to its respective target joint motion while satisfying its respective unified constraint. As a result, joints that are near their position limit are clamped at or new the limit, while other joints perform their target motion. Regardless, however, each joint’s movement will respect its corresponding unified constraint. This movement may be due to the fact that since the joints cannot perform a synchronized movement, they will then move according to their respective unified constraints. The system may update joint position data (at block 560). For instance, if this is the first iteration of the process 500, the system may modify the position data measured by the sensors of the robotic arm based on the new position of the joint. As an example, if the measured position data indicated that the joint was at 0.7 radians, and the new position of the joint is at 1.0 radians, which may be set to the joint position limit, then the system 100 may update the position of the joint to be 1.0 radians.

[0078] Turning back to decision block 540, if, however, the joint may not exceed the joint position limit, the system 100 may determine joint motion based on the target joint motion for moving the joints from their current positions, while satisfying (or maintaining) their respective unified constraints (at block 550). The trajectory planner 450 may be configured to determine the joint motions as (actual) jointAtorney Docket No. : AUR6374WOPCT2Electronically Filed velocities of one or more joints based on the target velocities of those joints from the joints’ current positions by solving an optimization problem. In particular, the trajectory planner may be configured to synchronously scale the target velocities of the joints by using each joint’s unified constraint that concurrently binds position, velocity, acceleration, and / or higher-order constraints, such as jerk associated with the joint’s current position together. In particular, scaling the target joint motion may ensure motion of the joint stays within one or more joint limits associated with the unified constraint. For instance, the planner 450 may determine, for each joint, a scaling factor for a respective joint motion (e.g., to be applied to a respective target joint motion) that prevents violation of (e.g., exceeding) any individual joint limit maintained by the unified constraint. As described herein, the planner may scale each of the target joint motions using a scaling factor that satisfies (e.g., maximizes) the feasible velocities of each joint’s unified constraint. In other words, the planner 450 may select a largest scaling factor of the determined scaling factors that ensures joint motion of the joints satisfy their respective unified constraint. The system determines the scaling factor (e.g., a gain between 0 and 1) that enables the unified motion across the joints based on all unified constraints, and applies the factor to the target joint motion. In one embodiment, the joint movements produced by the planner 450 may include target velocities of joints, which may be scaled, over a period of time and from each joint position. Thus, the joint movements may include one or more joint velocities over a given time period.

[0079] The system 100 updates the joint position data (at block 560). In particular, the joint movements produced by the trajectory planner 450 may indicate the joint velocity from current joint positions for a given time period, which satisfy unified constraints by being within the feasible velocity. In which case, the system 100 may determine each joint’s new position based on the joint’s determined movement according to the optimization process. For instance, when the current joint position of a joint is 0.5 radians, and the joint movement defined by the trajectory planner 450 indicates that the joint is to move 0.2 radians / second, when the time period includes one second, the new position of the joint may be 0.7 radians.

[0080] The system determines a new tool pose based on the updated joint position data (at block 565). In which case, the system may determine a resulting poseAtorney Docket No. : AUR6374WOPCT2Electronically Filed of the HID according to the determined (scaled) joint motions which provide the updated joint position data. This new pose may differ from the intermediate tool pose due to changes to the target joint velocities based on satisfying the unified constraints. The pose may also differ when one or more joint positions are clamped based on their proximity to their joint position limits, as described herein.

[0081] The system determines whether the new tool pose converges with the target tool pose (at decision block 570). The system may determine whether the new tool pose converges within a threshold of the target tool pose. If not, meaning that the new tool pose would not result in the tool 171 reaching its target pose, the system 100 would return to block 520 to determine another intermediate tool pose based on the target tool pose and the current tool pose that would be the new tool pose determined at block 565.

[0082] The system may iteratively perform operations 520-570 until a newest tool pose converges (e.g., within a threshold) with the targe tool pose. For instance, the system 100 determines a new tool pose error based on the updated joint position data, determines a new intermediate pose based on the new tool pose error, determines, based on new target joint motions to achieve the new intermediate pose, new joint motion to move joints that satisfy one or more new unified constraints, and determines a new resulting pose based on the new joint motion. The new unified constraints may be different than previously determined unified constraints, since they are based on the current joint position. As a result, the system 100 may produce a trajectory for each joint, where the trajectory may include a series of joint movements for one or more joints in order to move the robotic 110 into the target pose. For instance, the system may determine the trajectory by determining joints movements, which may include one or more joint velocities at one or more joint positions of the joints of the arm 110, which cause the pose of the arm to move along the original tool pose error, where the joint movements may be based on target joint motion that satisfies the joint’s unified constraint for moving the robotic arm from one pose to another. In one embodiment, the trajectory may include a data structure that associates joint motions for given joint positions. The data structure may indicate increments of time at which a joint is to move at a given velocity from its starting position. In another embodiment, the data structure may associate the joint positions of joints with respect to time. For instance,Atorney Docket No. : AUR6374WOPCT2Electronically Filed the system may store the new positions (updated joint position data) of the joints as the process 500 is performed, in order to indicate the path along which a joint is to travel over a given time period in order for the robotic arm to reach its target pose. Upon determining that the new tool pose converges, the system may output joint commands based on the joint position data to cause the robotic arm to move the surgical tool into the target pose (at block 575). In particular, the system may output one or more joint commands for at least some joints based on the joint’s determined trajectory to move the one or more of the joints of the robotic arm. Thus, the IK 420 may have trajectory planning embedded into the IK function, thereby eliminating the need for separate motion planning stages, reducing computational overhead, and enabling real-time operations. Moreover, the IK operations are capable of performing in real-time due to using efficient numerical methods that minimize computational latency.

[0083] As described herein, one or more joints may be clamped at or near their respective position limits based on whether joint motion will exceed those limits. Such operations may be performed when an operator commands the robotic arm 110 to move beyond its operating limits. As a result, the system 100 may be configured to alert the user when one or more joints are clamped. For example, the system may be configured to output a visual or audible alert through the display 150 or a speaker, respectively. In another embodiment, the system may output haptic feedback through the HID 140 to indicate that the robotic arm 110 has encountered one or more position limits. As a result, the user may then instruct the robotic arm 110 to move such that the system may unclamp the joints. For example, upon clamping a joint moving in one direction, the user may move the robotic arm such that the joint moves in the opposite direction.Once the joint motion is determined to move the joint in the opposite direction - away form the position limit at which the joint is clamped - the system may unclamp the joint and allow the joint motion.

[0084] Fig. 6 is a flowchart of another embodiment of a process 600 for performing constraint-aware IK with trajectory planning for joint movement. The process 600 begins with the system 100 receiving, via the HID, a target pose of an end effector of the robotic arm (at block 610). For instance, the target pose of the end effector may be based on a user command received through the HID based on user input of moving the end user control 220 of the HID 140. The system 100 determines aAtorney Docket No. : AUR6374WOPCT2Electronically Filed trajectory for moving at least one joint of several joints to cause the end effector to move to the target pose, where the trajectory includes a series of joint movements, each joint movement satisfying a unified constraint of the joint associated with a position of the joint from which the joint movement is to occur (at block 620). For instance, for each joint, the system 100 determines target joint motion (e.g., a joint velocity) to move the joint from its current position to cause the robotic arm to move towards a target pose, establishes a unified constraint based on the joint’s current position, and determines (actual) joint motion by scaling the target joint motion based on the established unified constraint, where the scaling maximizes synchronized movement of the joints dependent upon feasible joint motion of the joints. The system 100 outputs the trajectory to cause the robotic arm to move the end effector into the target pose (at block 630). As a result, the system ensures that motion of a joint may be adjusted, which would otherwise cause the joint to exceed at least one limit (e.g., position limit, joint velocity limit, and / or acceleration limit), such that the joint will stop at or before exceeding such limits when the trajectory is output.

[0085] Some aspects may perform variations to the processes 500 and / or 600 described herein. For example, the specific operations of at least some of the processes might not be performed in the exact order shown and described. The specific operations might not be performed in one continuous series of operations, and different specific operations might be performed in different embodiments. For instance, as described in Fig. 5, the system may clamp a joint’s position based on whether the target joint motion may exceed a joint limit. In another embodiment, this determination may be made after the scaling factor is applied to the target joint motion. For instance, the system 100 may determine whether at least one scaled joint motion causes a respective joint to exceed a joint position limit based on the current position of the respective joint, and in response, may update the joint position of the joint to its joint limit or at a threshold from the limit. This may ensure that joint motion does not result in a joint meeting or exceeding (e.g., within a threshold) its joint limit. In which case, upon determining the joint motion at block 550, the system 100 may be configured to determine the resulting joint motion by adjusting the scaled joint motion such that the joint is to stop at or before the joint position limit, responsive to determining that the joint movement is to cause the joint to come within a threshold or exceed a joint position limit. As a result, when theAtorney Docket No. : AUR6374WOPCT2Electronically Filed tool pose converges with the target tool pose, the system may generate a tool command for the clamped joint so that the clamped joint may move to or within the threshold of its respective joint position limit.

[0086] Figs. 7A-7C and 8A-8D illustrate examples of feasible joint velocities that would satisfy a joint’s unified constraint. In particular, Figs. 7A-7C illustrate examples of feasible joint velocities with respect to different ranges of velocity limits for trajectory planning, where the joint velocities satisfy (or do not exceed) at least one joint limit, such as position limits, velocity limits and / or other limits, such as acceleration limits. Each figure shows a plot of joint position, between -Q to +Q, with respect to velocity, -Eto + V of the joint. Each of these ranges may represent the joint position limit and the joint velocity limit, respectively, of the joint. The shaded area in each figure shows a feasible velocity, Vfeasibie, which may be determined by the trajectory planner when solving the optimization problem while satisfying the unified constraint, as described herein. In other words, Vfeasibie may represent the feasible joint motion indicated by a joint’s unified constraint of which the system may determine a joint motion. The dashed line represents an upper or maximum velocity, Vmax, while the dashed line represents a lower or minimum velocity, Vmin.

[0087] These figures illustrate changes to the feasible velocity based on changes (e.g., ever increases) to the velocity limit. Fig. 7A shows the feasible velocity between -Vi to + Vi, while Fig. 7B shows that the feasible velocity increases due to an increase in the velocity limit to -V2 to + V2. Fig. 7C shows another increase in the feasible velocity due to another increase in the velocity limit to -V3 to Vs. The slopes of Vmin and Vmax are shown as changing as the velocity limit increases. The position limits of each of these figures may stay the same, but the feasible velocity and the minimum and maximum velocities change. In one embodiment, other limits may remain constant between these figures, such as joint acceleration limit.

[0088] Figs. 8A-8D illustrate examples of feasible joint velocities for trajectory planning, where variations in the feasible velocity are based on changes to acceleration limits. Fig. 8A illustrates feasible velocities due to a first set of acceleration limits. Fig. 8B shows the changes to the velocity based on an expansion of those acceleration limits. For instance, this figure may show the feasible velocity based on an increase inAtorney Docket No. : AUR6374WOPCT2Electronically Filed the acceleration limits of 10%. Fig. 8C shows changes to feasible velocities based on another expansion of the acceleration limits, where the limits from Fig. 8A have been expanded by 100%. Finally, Fig. 8D shows a saturation of the feasible velocity due to the acceleration limits from Fig. 8A being expanded by 10,000%. As a result, these figures show how Vfeastbie changes due to changes to the combined limits of the unified constraint and shows velocities that do not conflict with other limits, such as the acceleration limit.

[0089] As previously explained, an embodiment of the disclosure may be a non-transitory machine-readable medium (such as microelectronic memory) having stored thereon instructions, which program one or more data processing components (generically referred to here as a “processor”) to (automatically) perform constraint- aware IK with trajectory planning operations, as described herein. In other embodiments, some of these operations might be performed by specific hardware components that contain hardwired logic. Those operations might alternatively be performed by any combination of programmed data processing components and fixed hardwired circuit components.

[0090] To aid the Patent Office and any readers of any patent issued on this application in interpreting the claims appended hereto, applicants wish to note that they do not intend any of the appended claims or claim elements to invoke 35 U.S.C. 112(f) unless the words “means for” or “step for” are explicitly used in the particular claim.

[0091] While certain embodiments 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 disclosure, and that the disclosure is not limited to the specific constructions and arrangements shown and described, since various other modifications may occur to those of ordinary skill in the art. The description is thus to be regarded as illustrative instead of limiting.

[0092] In some embodiments, this disclosure may include the language, for example, “at least one of [element A] and [element B].” This language may refer to one or more of the elements. For example, “at least one of A and B” may refer to “A,” “B,” or “A and B.” Specifically, “at least one of A and B” may refer to “at least one of A and at least one of B,” or “at least of either A or B.” In some embodiments, thisAtorney Docket No. : AUR6374WOPCT2Electronically Filed disclosure may include the language, for example, “[element A], [element B], and / or [element C] ” This language may refer to either of the elements or any combination thereof. For instance, “A, B, and / or C” may refer to “A,” “B,” “C,” “A and B,” “A and C,” “B and C,” or “A, B, and C.”

Claims

Atorney Docket No. : AUR6374WOPCT2Electronically FiledCLAIMSWhat is claimed is:

1. A method of a surgical robotic system that comprises a human interface device (HID) configured to control a robotic arm that comprises a plurality of joints, the method comprising: receiving, via the HID, a target pose of an end effector of the robotic arm; determining a trajectory for moving the plurality of joints to cause the end effector to move to the target pose, wherein the trajectory comprises a series of joint movements, each joint movement satisfying a unified constraint of the joint associated with a position of the joint from which the joint movement is to occur; and outputting the trajectory to cause the robotic arm to move the end effector into the target pose.

2. The method of claim 1 further comprising: determining an intermediate pose of the end effector between a starting pose of the end effector and the target pose of the end effector; and determining, for each joint, a target joint velocity for moving from a current joint position, wherein determining the trajectory comprises determining, for each joint, a joint velocity for the joint movement based on the target joint velocity that satisfies the unified constraint for moving the end effector from the starting pose into the intermediate pose.

3. The method of claim 2, wherein determining the trajectory comprises: determining, for each joint, whether the joint velocity of the joint is to cause the joint to exceed at least one of a joint position limit, a joint velocity limit, or a joint acceleration limit; and in response, adjusting a respective joint movement such that the respective joint is to stop at or before at least one of the joint position limit, the joint velocity limit, or the joint acceleration limit when the trajectory is output.Atorney Docket No. : AUR6374WOPCT2Electronically Filed4. The method of claim 2, wherein determining the intermediate pose of the end effector comprises: determining a pose error based on a difference between the starting pose and the target pose of the end effector; and applying a gain to the pose error based on the difference.

5. The method of claim 2 further comprising determining, for each joint, the unified constraint based on the current joint position, wherein determining the joint velocity for each joint comprises: determining a scaling factor that enables unified motion across the plurality of joints based on all unified constraints of the plurality of joints; and applying the scaling factor to the target joint velocity of the joint.

6. The method of claim 1, wherein each unified constraint accounts for at least one of a position limit of the joint, a velocity limit of the joint, or an acceleration limit associated with a current joint position of a respective joint.

7. The method of claim 1 further comprising: receiving position data of the plurality of joints; updating the position data of the plurality of joints based on the joint movements; and determining a resulting pose of the end effector based on the updated position data, wherein the trajectory is output responsive to a determination that the resulting pose of the end effector converges with the target pose of the end effector within a threshold.

8. The method of claim 7 further comprising iteratively, until the resulting pose of the end effector converges with the target pose of the end effector, determining an intermediate pose of the end effector between a previously determined resulting pose and the target pose of the end effector;Atorney Docket No. : AUR6374WOPCT2Electronically Filed determining, for each joint, a new joint movement based on a new target velocity that satisfies a new unified constraint for a current joint position of the joint; and determining the new resulting pose of the end effector based on the determined ne w j oint movements .

9. The method of claim 1, wherein determining the trajectory comprises: for each joint, determining whether the joint movement is to cause the joint to reach its respective joint position limit; and responsive to determining that a particular joint movement is to cause a respective joint to reach its respective joint position limit, adjusting the particular joint movement to cause the respective joint to stop at its respective joint position limit while still satisfying the unified constraint.

10. A surgical system comprising: a robotic arm that comprises a plurality of joints; a human interface device (HID) configured to control the robotic arm; at least one processor; and memory having instructions stored therein which when executed by the at least one processor causes the surgical system to: receive, via the HID, a target pose of an end effector of the robotic arm, determine, for each of the joints, a unified constraint of the joint based on a current position of the joint; and a joint motion from the current position that satisfies the unified constraint; and determine a trajectory for the plurality of joints based on respective joint motions for moving the end effector into the target pose.

11. The surgical system of claim 10, wherein the unified constraint for each joint comprises at least one of a position limit, a velocity limit, and an acceleration limit of the joint associated with the current position of the joint.Atorney Docket No. : AUR6374WOPCT2Electronically Filed12. The surgical system of claim 10, wherein the instructions to determine the trajectory comprises instructions to, responsive to a determination that the joint motion of a particular joint is to cause the particular joint to move to a joint position limit of the unified constraint, set a new position towards which the particular joint is to move to the joint position limit; and for each remaining joint, set a respective new position towards which the joint is to move according to its respective joint motion.

13. The surgical system of claim 10, wherein the memory comprises further instructions to determine a pose error between the target pose of the end effector and a starting pose of the end effector, wherein the joint motion of each joint is determined based on the pose error.

14. The surgical system of claim 13, wherein the memory comprises further instructions to determine an intermediate pose of the end effector by scaling the pose error based on a magnitude of the pose error, wherein instructions to determine the joint motion comprises instructions to: determine, for each joint, a target joint motion to cause the end effector to move towards the intermediate pose; and scale the target joint motion to ensure motion of the joint stays within one or more joint limits of the unified constraint.

15. The surgical system of claim 10, wherein the memory comprises further instructions to receive joint position data for the plurality of joints that indicate the current positions of the joints, wherein the instructions to determine the trajectory comprises instructions to update, for each joint, respective joint position data according to the joint motion from the current position of the joint.

16. The surgical system of claim 15, wherein the memory comprises further instructions to: determine a new pose of the end effector based on the updated joint position data; andAtorney Docket No. : AUR6374WOPCT2Electronically Filed output joint commands based on the updated joint position data to cause the end effector to move into the target pose responsive to the new pose converging with the target pose within a threshold.

17. The surgical system of claim 16, wherein the unified constraint is a first unified constraint, and the joint motion is a first joint motion, wherein the memory comprises further instructions to, responsive to the new pose failing to converge with the target pose within the threshold and until the new pose converges with the target pose within the threshold, for each of the joints, determine a second unified constraint of the joint based on a new position of the joint based on the updated joint position data, wherein the second unified constraint is different than the first unified constraint; determine a second joint motion from the new position that satisfies the second unified constraint; update respective joint position data according to the second joint motion; and update the new pose of the end effector based on the updated joint position data.

18. A method comprising : determining a current pose of a robotic arm that comprises a plurality of joints; determining a target pose of the robotic arm; for each joint of the plurality of joints, determining a joint motion to move the joint from a current position to cause the current pose of the robotic arm to move towards the target pose; and establishing a unified constraint based on the current position of the joint, wherein the unified constraint comprises one or more joint limits of the joint; scaling the joint motions of the plurality of joints based on established unified constraints; and provide joint commands to ensure synchronized motion between at least a portion of the plurality of joints based on the scaled joint motions.Atorney Docket No. : AUR6374WOPCT2Electronically Filed19. The method of claim 18 further comprising: determining a pose error based on a difference between the current pose and the target pose; and determining a new pose of the robotic arm based on pose error, wherein the joint motions of the plurality of joints are determined based on the new pose of the robotic arm.

20. The method of claim 19, wherein determining the new pose comprises applying a gain to the pose error based on a magnitude of the pose error.

21. The method of claim 18 further comprising determining a resulting pose of the robotic arm according to the scaled joint motions of the plurality of joints, wherein the joint commands are provided responsive to the resulting pose converging with the target pose within a threshold.

22. The method of claim 21 further comprising iteratively, until the resulting pose of the robotic arm converges with the target pose within the threshold, determining, for each joint, a new joint position based on a previously determined resulting pose of the robotic arm; a new unified constraint based on the new joint position; and a new joint motion to move the joint from the new joint position while satisfying the new unified constraint; and determining a new resulting pose of the robotic arm based on the new joint motions.

23. The method of claim 18, wherein the unified constraint maintains the one or more joint limits that comprises at least one of a joint position limit, a joint velocity limit, an acceleration limit, or a jerk limit.

24. The method of claim 18 further comprising: determining, for each joint, a scaling factor for a respective joint motion that prevents violation of any individual joint limit maintained by the unified constraint; andAtorney Docket No. : AUR6374WOPCT2Electronically Filed selecting a largest scaling factor of the determined scaling factors that ensures joint motion of the plurality of joints satisfy their respective unified constraint, wherein scaling the joint motions comprises applying the largest scaling factor to each joint motion.

25. The method of claim 18 further comprising: determining whether at least one scaled joint motion causes a respective joint to exceed at least one of a joint position limit, a joint velocity limit, or a joint acceleration limit based on the current position of respective joint; and in response, generating a respective joint command for the respective joint to move to or within a threshold of the joint position limit, the joint velocity limit, or the joint acceleration limit.

Images