GB2625795A - Joint control unit - Google Patents
Joint control unit Download PDFInfo
- Publication number
- GB2625795A GB2625795A GB2219710.7A GB202219710A GB2625795A GB 2625795 A GB2625795 A GB 2625795A GB 202219710 A GB202219710 A GB 202219710A GB 2625795 A GB2625795 A GB 2625795A
- Authority
- GB
- United Kingdom
- Prior art keywords
- joint
- drive source
- control unit
- configuration signal
- torque
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000005484 gravity Effects 0.000 claims abstract description 11
- 238000012546 transfer Methods 0.000 claims abstract description 6
- 230000009471 action Effects 0.000 claims abstract description 5
- 238000000034 method Methods 0.000 claims description 25
- 238000003860 storage Methods 0.000 claims description 14
- 210000001503 joint Anatomy 0.000 description 26
- 230000033001 locomotion Effects 0.000 description 16
- 239000012636 effector Substances 0.000 description 15
- 238000005259 measurement Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 9
- 238000004891 communication Methods 0.000 description 8
- 210000003414 extremity Anatomy 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 238000005070 sampling Methods 0.000 description 6
- 238000013459 approach Methods 0.000 description 5
- 206010023230 Joint stiffness Diseases 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 210000000707 wrist Anatomy 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000011217 control strategy Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000035945 sensitivity Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 241000282375 Herpestidae Species 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 210000000080 chela (arthropods) Anatomy 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005489 elastic deformation Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000004424 eye movement Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 239000000383 hazardous chemical Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000010348 incorporation Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000002085 persistent effect Effects 0.000 description 1
- 238000007639 printing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000002834 transmittance Methods 0.000 description 1
- 230000007723 transport mechanism Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
- A61B34/37—Master-slave robots
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/70—Manipulators specially adapted for use in surgery
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/70—Manipulators specially adapted for use in surgery
- A61B34/74—Manipulators with manual electric input means
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
- A61B2034/2046—Tracking techniques
- A61B2034/2059—Mechanical position encoders
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/70—Manipulators specially adapted for use in surgery
- A61B34/74—Manipulators with manual electric input means
- A61B2034/742—Joysticks
Landscapes
- Health & Medical Sciences (AREA)
- Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Surgery (AREA)
- Robotics (AREA)
- Medical Informatics (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- General Health & Medical Sciences (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
A control unit for a surgical robot 100, the surgical robot 100 comprising a joint 106a-e driven by a drivetrain which transfers power from a drive source such as a motor 114 to the joint 106a-e, in dependence on a drive source configuration signal indicating a position of the drive source and a joint configuration signal indicating a position of the joint 106a-e in the surgical robot 100, the control unit 118 calculates a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range and outputs the value of reference torque to a joint controller (402 figure 4) for controlling the joint 106a-e. The frequency components may be the outputs from a high pass filter and a low pass filter. The configuration signals may be determined from data acquired by position sensors 116 and may be measured angular positions. Calculating the value of reference torque may comprise adding a torque term which compensates for the action of gravity on the joint.
Description
JOINT CONTROL UNIT
FIELD OF THE INVENTION
This invention relates to controlling the configuration of a joint in a mechanical system, such as a joint in the arm of a surgical robot
BACKGROUND
A typical robotic manipulator comprises a series of rigid elements that are coupled together by one or more joints, which may be joined in series to form an arm. The manipulator further comprises a base unit at a first, proximal end of the series of rigid elements, and an end effector at a second, distal end that opposes the first end. Each joint of the manipulator is driven by a drive source and a corresponding drivetrain so as to cause relative motion of the rigid elements. This relative motion is used to vary the configuration of the end effector at a desired location. Each joint may provide rotational motion and/or linear motion. The drive source may be any suitable means, such an electronic motor or a hydraulic actuator.
In theoretical modelling, the drivetrain that actuates a robotic joint can be modelled as "stiff". A stiff drivetrain is one that does not experience any elasticity or backlash. Backlash is experienced when the drivetrain changes direction and is caused by gaps or clearance between interfacing components within the drivetrain. If there is no elasticity or backlash in the drivetrain, and if the drivetrain is frictionless, massless and infinitely stiff, then the torque generated by the drive source (i.e. the input torque that is provided to a first end of the drivetrain) will always be proportional to the torque that is experienced by the joint (i.e. output torque at a second end of the drivetrain that opposes the first end).
In reality, the components of the drivetrain that actuate a robotic joint have an associated level of elasticity, mass and friction. Thus, the dynamics of the joint cannot be accurately modelled using a linear relationship between input torque and output torque. There is a dynamic relationship (i.e. one described by differential equations) between drive source torque and output torque. Therefore, a real joint controller controls both the internal dynamics of the joint and the external dynamics of the arm.
It is therefore necessary to control the drive source using an algorithm that incorporates multiple feedback terms, those feedback terms corresponding to sensor data measured at both the input and output of the drivetrain. Sensor data can therefore be used to provide an indication of the performance of the joint. The incorporation of multiple feedback terms into a control algorithm increases the complexity of a controller associated with that algorithm and also introduces stability and accuracy limitations.
There is a need for a method of controlling the configuration of a joint in a mechanical system, such as a robot arm, that provides good robustness and performance.
SUMMARY OF THE INVENTION
According to a first aspect, there is provided a control unit for a surgical robot, the surgical robot comprising a joint driven by a drivetrain which transfers power from a drive source to the joint, wherein the control unit is configured to: in dependence on a drive source configuration signal indicating a position of the drive source and a joint configuration signal indicating a position of the joint in the surgical robot, calculate a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range; and output the value of reference torque to a joint controller for controlling the joint.
The joint controller may modulate a current input to the drive source to achieve the reference torque at the drive source (i.e. so that the drive source torque matches the reference torque).
The control unit may be configured to receive the drive source configuration signal indicating a position of the drive source. The control unit may be configured to receive the joint configuration signal indicating a position of the joint in the surgical robot. The control unit may be configured to receive the frequency components of the drive source configuration signal that are within the high frequency range. The control unit may be configured to receive the frequency components of the joint configuration signal that are within the low frequency range. The control unit may be configured to receive a signal comprising the frequency components of the drive source configuration signal that are within the high frequency range and the frequency components of the joint configuration signal that are within the low frequency range.
The control unit may calculate the value of reference torque from the received signal(s) or components.
The frequency components of the drive source configuration signal that are within the high frequency range may be the output of a high pass filter taking the drive source configuration signal as input. The frequency components of the joint configuration signal that are within the low frequency range may be the output of a low pass filter taking the joint configuration signal as input. The control unit may be configured to calculate the value of the reference torque using frequency components of the drive source configuration signal that are output from a high pass filter taking the drive source configuration signal as input and frequency components of the joint configuration signal that are output from a low pass filter taking the joint configuration signal as input.
The control unit may therefore be configured to determine the frequency components of the drive source configuration signal and/or the frequency components of the joint configuration signal by applying one or more filters to the respective signals. For example, the control unit may be configured to apply a low pass filter to the joint configuration signal and a high pass filter to the drive source configuration signal.
The respective filters may take the drive source configuration signal or the joint configuration signal as input. The drive source configuration signal and/or the joint configuration signal may comprise raw sensor data, or the raw sensor data may be scaled or input to some other filter before the drive source configuration signal or the joint configuration signal is output.
The one or more filters (for example, the high pass filter and/or the low pass filter) may each apply a different gain to different frequency components of the respective signals.
The control unit may be configured to calculate the value of reference torque using a position signal given by got, + cal L), where q, is the joint configuration signal, qi is the drive source configuration signal and L is a low pass filter. Here, (1 -L) is a high pass filter.
The control unit may be further configured to receive a reference input indicating a desired position of the joint.
The value of reference torque may be further calculated using the reference input indicating a desired position of the joint.
The value of reference torque may be calculated using a difference between a first derivative of the reference input and a first derivative of the drive source configuration signal. The derivatives may be with respect to time.
The value of reference torque may be calculated using a virtual position signal. The value of reference torque may be calculated using a difference between the reference input and the virtual position signal. The virtual position signal may be calculated using the frequency components of the drive source configuration signal that are within the high frequency range and frequency components of the joint configuration signal that are within the low frequency range. The virtual position signal may be given by qvirt = clot, ± q1(1-a where q, is the joint configuration signal, qi is the drive source configuration signal and L is a low pass filter (and therefore (1-L) is a high pass filter).
The value of the reference torque may be given by the equation: Tref =Tgrav Kp(gref qv)+ K(wref a)i)' where grin = q0L + qi(1-L) and where (Ler is the reference input, coref is a reference angular velocity, q, is the joint configuration signal, qi is the drive source configuration signal, col is the angular velocity for the drive source configuration signal, rp,." is a gravitational torque term, E is joint efficiency, Kp is a proportional gain to the control unit, Kd,i is a derivative gain being applied to the drive source configuration signal and L is a low pass filter.
The drive source configuration signal may be determined from data acquired by a first position sensor. The joint configuration signal may be determined from data acquired by from a second position sensor.
The first position sensor may be located at, or at a position proximal to, a first end of the drive train at which the drive source is located. The second position sensor may be located at, or at a position proximal to, a second end of the drive train at which the joint is located.
The drive source configuration signal may be the measured angular position of the drive source. The joint configuration signal may be the measured angular position of the joint.
Calculating the value of reference torque may comprise adding a torque term which compensates for the action of gravity on the joint.
Calculating the value of reference torque may comprise adding a further torque term when the value of an externally applied torque at the joint exceeds a predetermined value.
The value of the further torque term may be determined in dependence on a function where: < Tirl*ht! T1 < -ri,141.1" put and where T is the predetermined where + value. erise.d torque at Ui The high frequency range may correspond to frequencies of greater than a first frequency value between approximately 1 to 3 Hz. The low frequency range may correspond to frequencies of less than a second frequency value between approximately ito 3 Hz. For example, the high frequency range may predominantly contain frequencies that are greater than a threshold of 3 Hz and the low frequency may predominantly contain frequencies that are less than the threshold of 3Hz. The high frequency range and the low frequency range may partially overlap. For example, the high and low frequency ranges may both contain frequencies that are within the range 2.5-3.5 Hz. The high frequency range may have a median (or other average) frequency that is higher than the low frequency range. The lowest frequency value within the low frequency range may be less than the lowest frequency value within the high frequency range. This may reflect the distribution of frequency components of signals that are output from a high pass filter (in the case of the high frequency range) and a low pass filter (in the case of the low frequency range). Such filters may have a specified cut-off frequency (for example, 3 Hz) and attenuate signals with frequencies above (for a low pass filter) and below (for a high pass filter) the cut-off frequency, but are not true step functions. The lowest frequency of the low frequency range may be 0 Hz. The highest frequency of the high frequency range may be dependent on the sensitivity of the sensor that acquired the signal data and/or the frequency at which the signal is transmitted. For example, this may be 5 Hz. In other implementations, this may be up to 50 Hz. ;In some implementations, the control unit may be configured to calculate a value of reference torque for the joint using frequency components of the drive source configuration signal that are above a frequency threshold and frequency components of the joint configuration signal that are below a frequency threshold. These thresholds may be different. ;The control unit may be configured to repeatedly calculate the value of reference torque. ;The drive source may be a motor. ;According to a second aspect, there is provided a method (e.g. a computer-implemented method) for controlling a surgical robot, the surgical robot comprising a joint being driven by a drivetrain which transfers power from a drive source to the joint, the method comprising: in dependence on a drive source configuration signal indicating a position of the drive source and a joint configuration signal indicating a position of the joint in the surgical robot, calculating a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range; and outputting the value of reference torque to a joint controller for controlling the joint. ;According to another aspect, there is provided a computer-readable storage medium have stored thereon computer readable instructions that when executed at a computer system comprising one or more processors cause the one or more processors to perform the methods above. The computer-readable storage medium may be a non-transitory computer-readable storage medium. ;BRIEF DESCRIPTION OF THE FIGURES ;The present invention will now be described by way of example with reference to the accompanying drawings. In the drawings: Figure 1 schematically illustrates an example of a robot arm. ;Figure 2 is a schematic block diagram illustrating an idealised control loop for controlling the configuration of a joint in a robot arm Figure 3 illustrates the characterisation of the non-linear stiffness of a drivetrain for a robotic joint. ;Figure 4 is a schematic block diagram illustrating a full state control scheme for controlling the configuration of a joint in a robot arm. ;Figure 5 is a schematic block diagram illustrating one implementation of a control scheme for controlling the position of a joint in a robot arm. ;Figure 6 is a schematic block diagram illustrating another implementation of a control scheme for controlling the position of a joint in a robot arm. ;Figure 7 is a schematic block diagram illustrating a further implementation of a control scheme for controlling the position of a joint in a robot arm. ;Figure 8 schematically illustrates the variation of a function 0 with output torque Figure 9 shows the steps of a method for controlling a joint in a surgical robot. ;Figure 10 is a schematic block diagram illustrating components of an exemplary 25 computing-based device. ;DETAILED DESCRIPTION ;The following description describes the present techniques within the context of robotic systems. Robotic systems can include manufacturing systems, such as vehicle manufacturing systems, parts handling systems, laboratory systems, and manipulators such as for hazardous materials or surgical manipulators. The robotic system illustrated in Figure 1 is specifically a surgical robotic system. However, the features described below are not limited to such a system but are applicable to robotic systems more generally. ;Figure 1 illustrates a surgical robot having an arm 100 which extends from a base unit 102. The arm comprises a plurality of rigid limbs 104a-e which are coupled by a plurality revolute joints 106a-e. The revolute joints 106a-e are configured to apply rotational motion. Instead of or in addition to these revolute joints, the surgical robot may comprise a one or more joints for providing linear motion. The limb that is closest to the base 102 is the most proximal limb 104a and is coupled to the base by a proximal joint 1062. The remaining limbs of the arm are each coupled in series by a joint of the plurality of joints 106b-e. A wrist 108 may comprise four individual revolute joints. The wrist 108 couples one limb (104d) to the most distal limb (104e) of the arm. The most distal limb 104e carries an attachment 110 for a surgical instrument 112. Each joint 106a-e of the arm 100 has one or more drive sources 114 which can be operated to cause rotational motion at the respective joint. Each drive source 114 is connected to its respective joint 106a-e by a drivetrain which transfers power from the drive source to the joint. In one example the drivetrain comprises one or more gears. In a more specific example, the drivetrain is a harmonic drive. A harmonic drive comprises the concentric arrangement of a wave generator, a flex spline and a circular spline, and increases the torque that can be applied to the joint by the drive source. ;In one example, the drive sources 114 are motors. The drive sources 114 may alternatively be hydraulic actuators, or any other suitable means. Each joint 106a-e further comprises one or more configuration and/or force sensor 116 which provides information regarding the current configuration and/or force at that joint. Where the joint is a revolute joint, one or more of the sensors 116 may be a torque sensor. Where the joint is configured to provide linear motion, one of the one or more sensors 116 may be a strain gauge. In addition to configuration and/or force data, the one or more sensors 116 may additionally provide information regarding the temperature, current or pressure (such as hydraulic pressure). In one example, the one or more configuration sensors are position sensors. That is, the configuration sensors may measure the physical position of the joint. In such an example, one or more of the sensors 116 may be an encoder. An encoder is a position sensor which transforms a position measurement into an electronic signal. The encoder 116 may be a linear or a rotary encoder. ;The drive sources 114 are arranged proximally of the joints whose motion they drive, so as to improve weight distribution. For clarity, only some of the drive sources and sensors are illustrated in figure 1. It can also be appreciated that the surgical robot 100 may comprise more or less limbs and joints than those illustrated in figure 1. ;The instrument 112 of the surgical robot 100 comprises an end effector for performing an operation. The end effector may take any suitable form. For example, the end effector may be smooth jaws, serrated jaws, a gripper, a pair of shears, a needle for suturing, a camera, a laser, a knife, a stapler, a cauteriser or a suctioner. The instrument further comprises an instrument shaft and an articulation located between the instrument shaft and the end effector. The articulation comprises several joints which permit the end effector to move relative to the shaft of the instrument. ;Configuration controllers for the drive sources 114 and sensors 116 are distributed within the robot arm 100. The controllers are connected via a communication bus to a control unit 118. The control unit 118 comprises a processor 120 and a memory 122. The memory 122 stores, in a non-transient way, software that is executable by the processor 120 to control the operation of the drive sources 114 to cause the arm 100 to operate. In particular, the software can control the processor 120 to cause the drive sources (for example via distributed controllers) to drive in dependence on inputs from the sensors 116 and from a surgeon command interface 124. The control unit 118 is coupled to the drive sources 114 for driving them in accordance with outputs generated by execution of the software. The control unit 118 is electrically connected to the sensors 116 for receiving sensed input from the sensors, and to the command interface 124 for receiving input from it. The electrical connections described with reference to figure 1 and all subsequent figures may, for example, each be electrical or optical cables, or may be provided by a wireless connection. ;The command interface 124 comprises one or more input devices whereby a user can request motion of the end effector in a desired way. The input devices, or input controllers, could, for example, be manually operable mechanical input devices such as control handles or joysticks, touch-operable inputs such as touchscreens, or contactless input devices such as optical gesture sensors or voice sensors. The input devices might monitor eye movement to receive an input. The input devices could, for example, be some combination of these types of input devices. Commands input by the input devices can include movement commands, for example to move the instrument in a particular way, such as a lateral movement and/or a rotation. Such commands can include end effector commands, for example to control an end effector coupled to a distal end of the instrument 112 to operate the end effector, such as to open/close gripper jaws or to operate (turn on or off) an electrosurgical end effector. ;The software stored in the memory 122 is configured to respond to those inputs and cause the joints of the arm and instrument to move accordingly, in compliance with a pre-determined control strategy. The control strategy may include safety features which moderate the motion of the arm and instrument in response to command inputs. Thus, in summary, a surgeon at the surgeon console which suitably comprises the command interface 124 can control the instrument 112 to move in such a way as to perform a desired surgical procedure. Thus, the robot arm 100 functions as a master-slave manipulator where the control unit 118 acts as a master controller. The control unit 118 and/or the command interface 124 may be remote from the arm 100. ;Figure 2 illustrates an exemplary control loop 200 for controlling the configuration of a revolute joint in a robot arm as illustrated in figure 1. The control loop comprises a configuration controller 202, a torque controller 204, an inertia output 206 and a sensor 30 208 The controller 202 may be distributed within a robot arm, such as the arm illustrated in figure 1, such that it is proximal to a joint that it is configured to control. The configuration controller 202 is electrically connected to the one or more sensor 208 by means of a feedback loop. The feedback loop enables the controller 202 to receive measurements indicating the configuration of the joint from the sensor. The configuration controller 202 is also connected to the control unit 118 by means of a feedforward loop and is configured to receive a reference configuration value gref from the control unit 118. In one example, the configuration controller 202 is a separate entity to the control unit 118. In another example, the configuration controller 202 and the control unit 118 are comprised within the same entity. Thus, in this example, the control unit may be directly connected to the sensor 208. ;The reference configuration qv,: may otherwise be referred to as the desired or commanded configuration of the joint. The reference configuration q"f is calculated from an end effector pose that is commanded by the surgeon at the command interface 124. The reference configuration q"f may be calculated by applying one or more inverse kinematic calculations to the commanded end effector pose. In one example, the reference configuration q"f is the desired physical position of the joint. ;The configuration controller 202 is configured to perform a calculation using the reference position value q"f received from control unit 118 and the feedback value from the sensor 208 to generate a reference torque value ref. The reference torque value Tref indicates the demanded torque that is required to move the joint to the reference configuration q" f. The reference torque value x"f is then transmitted to the torque controller 204. ;The torque controller 204 is electrically connected to the configuration controller 202 and is configured to receive the reference torque value Tref. From this value of T _ref, the joint torque controller is configured to calculate a value of desired current that is to be generated by the drive source. This desired current is used to generate an input torque Ti, which is the actual torque applied by the drive source. Where the drive source is a motor, this may be termed the motor torque. The joint torque controller modulates a current input to the drive source to achieve the reference torque at the drive source (i.e. so that the drive source torque matches the reference torque). ;A joint that is not stiff, when it is driven, experiences an inertia which opposes the torque provided by the drive source. The inertia may be caused by a number of factors that are either internal or external to the joint. An exemplary cause of inertia is the weight of the joint due to gravity. Another exemplary cause is the weight of an object being supported by or interfacing with the joint. Due to presence of inertia, the configuration of the joint at its output q, may differ from the reference configuration For a joint that is stiff, the inertia will not have an effect on the output configuration of the joint q". Thus, for a stiff joint, there will be no difference between the output configuration q, and the desired configuration (Ler Sensor 208 is provided to measure the output configuration q, of the joint. The sensor 208 may correspond to one of the sensors 116 illustrated in figure 1. Thus, the sensor 208 may be comprised within a joint of the robot arm 100. In one example the output configuration q, is the output position of the joint. The sensor 208 may therefore be a position sensor such as a linear or a rotary encoder. In this example, the sensor 208 is configured to measure the position of the joint within which it is comprised, and to transmit an electrical signal indicating this measurement to the configuration controller 202. The transmittance of this electrical signal forms a feedback loop that is used by the configuration controller 202 to calculate an updated value reference torque Tref. ;Generally, feedback terms are used within control systems to indicate a deviation between the desired configuration of a joint and its actual configuration, and to compensate for disturbances that may take the joint further from steady state. Thus, the value of reference torque Tref that incorporates feedback from the sensor 208 may be continuously updated as continuous data measurements are obtained from the sensor 208. The feedback loop may further comprise one or more filters, integrators or differentiators for manipulating the raw output value that is obtained from the sensor 208 before it is provided to the configuration controller 202. ;As mentioned above, the joint to be controlled by the loop illustrated in figure 2 has been modelled as "stiff". For stiff joints, the input torque (i.e. the torque that is measured at, or at a position proximal to, a first end of the drive train at which the drive source is located) is proportional to the output toque (i.e. the torque that is measured at, or at a position proximal to a second end of the drive train at which the joint is located). ;The output torque measured about the joint can be characterised by the following equation: -c" = K(qi -q") where K is the spring constant, or stiffness, of the drivetrain, qi is the input configuration of the joint and q, is the output configuration of the joint. In one example, qi is measured at the first end of the drivetrain and q, is measured at the second end of the drivetrain. Thus, the output torque is characterised by a relationship between the elongation (qi -q,), and stiffness K of the drivetrain. In some examples, the relationship between elongation and stiffness K is linear. In other examples, this relationship is non-linear. ;The spring constant K referenced herein may otherwise be referred to as a spring coefficient. It is noted that the spring constant (or spring coefficient) does not necessarily have a constant value for all elongation values. Indeed, in some examples, the spring coefficient is a continuous function of the elongation value, and in some other examples the spring coefficient is selected from a set of discrete values, with each discrete value being associated with a respective range of elongation values. ;As described above, mechanisms for driving joints within a robotic system comprise an associated element of elasticity, which results in a non-linear stiffness characteristic. This elasticity is especially characteristic of harmonic drives. Figure 3 is a graph illustrating an exemplary non-linear stiffness characteristic for an elastic joint in a robot arm. The graph plots elongation on the x-axis and output torque on the y-axis. The output torque may be measured by a sensor located at a second end of the drivetrain, at which the joint is located. The sensor for measuring output torque may be a gauge sensor. A gauge sensor is configured to measure changes in voltage due to mechanical strain. Elongation is calculated by subtracting the input configuration qi from the output configuration qo of the joint. The input configuration can be measured by a first sensor located at a first end of the drivetrain at which the drive source is located. Output configuration can be measured by a second sensor located at a second end of the drivetrain at which the joint is located. ;In figure 3, output torque To is measured in Nm and elongation E is measured in degrees. In an alternative example, elongation E may be measured in radians. In both of these examples, the first and second sensors for measuring the input and output configurations are rotary position sensors. In a further example, elongation may be a linear measurement that may be calculated using measurements obtained from linear position sensors. In this example, elongation may be measured in meters. In a further example, elongation may be dimensionless. An example of a dimensionless measurement of elongation is as a percentage. ;The graph illustrated in figure 3 comprises five distinct regions 302-310. Regions 302 and 310 demonstrate a directly proportional relationship between drivetrain elongation and output torque I o. That is, as the elongation of the drivetrain increases, the output torque increases at a constant rate. This represents the idealised performance of the joint as described above with reference to figure 2. Region 302 illustrates the joint performance for negative values of elongation, and region 310 illustrates its performance for positive values of elongation. The gradient of the graph plotted in regions 302 and 310 represents the stiffness K of the drivetrain within the associated range of elongation values. ;Region 306 represents the phenomenon of backlash, where the gradient of line representing the stiffness of the drivetrain is near-zero. As described above, backlash refers to the lost motion in a mechanism that is experienced when it changes direction and is caused by gaps or clearance between interfacing components of the drivetrain. ;For a harmonic drive, backlash is the result of clearance between the circular spline and the flex spline, and/or elastic deformation of the flex spline under torque. This phenomenon typically occurs within a region of elongation values. In the example illustrated in figure 3, this region of elongation values is between -0.1 and 0 degrees. ;It will be appreciated that, for different joints, the backlash region will present itself at a different range of elongation values. In this region of the graph the measured sensor torque is zero and remains at zero as the elongation increases between -0.1 and 0 degrees. Thus, during the reversal of the joint direction between -0.1 and 0 degrees of elongation no torque is transmitted through the drivetrain from the drive source to the joint. ;Regions 304 and 308 are transition zones. In these zones the drivetrain is transitioning between the backlash region and its regions of intended performance. During these regions, there is a non-linear relationship between elongation and sensor torque. ;The representative stiffness of the drivetrain throughout the range of elongation values recorded in figure 3 is represented by the gradient of a regression line 312 that passes through all regions of the graph. It is appreciated from figure 3 that, for an elastic joint, the relationship between elongation and output torque is not entirely linear. That is, the gradient of the stiffness characteristic in the backlash region 306 (which is zero) differs substantially from its gradient in regions 302 and 310. Thus, the straight line represented by 312 does provide an accurate representation of the stiffness characteristic of the drivetrain for all values of elongation. ;The performance of joints demonstrating non-linear characteristics as illustrated in figure 3 cannot be accurately modelled. It is therefore important for robotic control systems to incorporate feedback loops that measure the real-world performance of the joint and use those performance measurements as feedback terms to adjust the reference signals that are used to drive the joint. ;The differential equations for a single elastic joint may be expressed as follows: Mcio + D(cio -41) + K(q° -qi) + F, = Belt D(cit c10)+ K (qi go) + F = To In the above equations, 114,B, K,D, Fi and Fo are parameters of the system being controlled M is the inertia at the output of the drivetrain, 130 is the inertia at its input (i.e. at the output of the drive source), K is the spring constant for the drivetrain, and D is a damping term. D, therefore, represents the energy lost due to the elasticity in the drivetrain. F1 is the input torque disturbance. Input torque disturbance may be due to static friction, for example. Fo is the output torque disturbance. In one example, output torque disturbance is caused by the other joints in the robot arm. ;In addition to the abovementioned control variables, the differential equations observe four state variables for an elastic joint: position, velocity, acceleration and torque. ;These variables may be defined at either the input or output of the drivetrain. In the above equations the output configuration of the joint qu is an output position of the joint. Thus, qo and 40 are the first and second derivatives of the output position, respectively, with respect to time. That is, q0 and eio are the velocity and acceleration, respectively, at the second end of the drivetrain. Correspondingly, qi, 4, and q, are the position, velocity and acceleration at the first end of the drivetrain. The differential equations above can be parameterised by measuring the state variables described above using position and torque sensors located the first and second ends of the drivetrain. Velocity and acceleration are derivatives of position with respect to time, and so can be calculated from position measurements measured from position sensors. Torque data values are traditionally obtained from torque sensor measurements. ;Figure 4 is a block diagram illustrating a full state control scheme for controlling the configuration of a joint in a robot arm. In the specific example illustrated in figure 4, the configuration of the joint is its position. The control scheme may be comprised within the joint controllers described with reference to figure 1. The control scheme comprises a joint controller 402 and a control loop 404 that are electrically connected to each other by interconnect circuitry. The joint controller 402 comprises one or more electrical connections for connecting to the joint 406 of the robot arm. The robot arm may correspond to the arm 100 illustrated in figure 1, and the joint 406 may correspond one or more of joints 104a-e. ;The joint controller 402 is configured to calculate a value of input torque "Li to be generated by the drive source and transmitted by the drivetrain to the joint 406. The control loop 404 is configured to receive a sensor data from the joint 406 and to calculate a reference torque value T"f Using this sensor data as feedback. ;The joint controller 402 comprises a drive source 408 and a drive source controller 410. The joint controller 402 controls torque and reads and/or processes sensor signals. The joint controller modulates the drive source (for example, the motor) current to achieve the reference torque at the drive source, i.e. so that the input torque at the drive source matches the reference torque. The drive source 408 is connected to both the joint 406 of the robot arm and to the drive source controller 410. The drive source 408 is configured to supply an input torque -ci to the drivetrain that drives the joint 406 of the robot arm. In one example, the drive source 408 is a motor. In other examples the drive source 408 may be a hydraulic or pneumatic actuator, or any alternative power source. The drive source controller 410 is configured to control the performance of the drive source, and therefore the input torque about the motor The drive source 408 and the drive source controller 410 are electrically connected to each other by means of a loop passing from the controller to the drive source, and a feedback loop passing from the drive source to the controller. In an example where the drive source is a motor, the loop passing from the controller to the drive source enables the controller to provide a three-phase voltage supply to the motor. In this example, the feedback loop enables the motor to supply a three-phase current feedback to the controller. The feedback loop is used to modify the voltage, and therefore the current, that is supplied to the motor. Thus, the feedback loop ensures that the intended torque is being generated by the motor. ;The joint controller 402 further comprises a plurality of sensors for measuring the one or more parameters indicating the performance of the joint 406. The sensors are further configured to generate electrical signals associated with their measurements, such that these electrical signals can be transmitted to the control loop 404. ;In one example, the sensors are comprised within the joint 406. In another embodiment the sensors are external, but coupled, to the joint 406. The plurality of sensors may correspond to the sensors 116 illustrated in figure 1. In figure 4, the plurality of sensors comprises a first sensor 412, a second sensor 414 and a third sensor 416. ;The first sensor 412 and the second sensor 414 are configured to measure the configuration about the joint 406. As mentioned above, the configuration of the joint may be the position of the joint. In this example, the sensors may be encoders. The encoders may be either linear encoders or rotary encoders, depending on the motion of the joint to which they are coupled. The first sensor 412 is configured to measure the input configuration of the joint 406. The input position is measured at the output of the drive source that is configured to drive that joint. Thus, the first sensor 412 may be located at, or at a position proximal to, a first end of the drivetrain, at which the drive source 408 is located. The second sensor 414 is configured to measure the output configuration at the joint 406. The output configuration is measured at the input to the joint. Thus, the second sensor is located at, or at a position proximal to, a second end of the drivetrain, at which the joint 406 is located. The third sensor 416 is configured to measure the output torque about the joint 406. Thus, the third sensor is also located at the second end of the drivetrain. The sensors 412, 414, 416 may be configured to continuously sense data indicating the performance of the joint. This continuous sensing of data may be performed at regular intervals. That is, the sensing of data from the joint may be performed by the sensors at a predefined sampling rate. This sampling rate may be configurable, such as at the command interface 124 illustrated in figure 1. ;In addition to the one or more sensors, the joint controller 402 may comprise one or more sensor filters for filtering raw electrical signals received from a respective sensor to form the signals sent to the controller. Filters can be provided to combine information from multiple data values to efficiently monitor the performance of the joint. In one example, the filters are temporal filters, which are used to incrementally update their information for each new data value. The first filter 418 is configured to filter electrical signals received from the first sensor 412. In one example, where the first sensor 412 is an input position sensor, the first filter 418 is configured to filter input position values. Correspondingly, a second filter 422 is electrically connected to the second sensor 414 and is configured to filter electrical signals received from that sensor. Thus, where the second sensor 414 is an output position sensor, the second filter 422 is configured to filter output position values. A third filter 426 is electrically connected to the third sensor 416. The third filter 426 is therefore configured to filter output torque values. ;The joint controller 402 may further comprise fourth and fifth filters 420, 424 for deriving filtered derivative values from the first and second sensors 412, 414 respectively. As mentioned above, the sensing of data by the sensors 412, 414, 416 may be performed at a predefined sampling rate. The fourth and fifth filters 420, 422 may be configured to maintain a record of the sampling rate at which sensor data is measured. Velocity (w) is the first derivative of position (q) with respect to time. Thus, where the first and second sensors 412, 414 are input and output position sensors, the fourth and fifth filters 420, 422 are configured to calculate input and output velocity values for the joint, respectively, using the predefined sampling rate and the continuously sampled data values. ;As well as being electrically connected to the one or more sensors, the one or more sensor filters are electrically connected to incoming interconnect circuitry 428. The incoming interconnect circuitry 428 is further connected to the control loop 404. The interconnect circuitry 428 is therefore configured to transmit sensor data from the joint controller 402 to the control loop 404. Thus, the sensor data may be provided as feedback to the control loop 404. In one example, the interconnect circuitry is a communication bus, such as an Ethercat Bus. ;The control loop 404 is configured to calculate a reference torque Tref to be provided to controller 410 and on to the drive source 408. The reference torque Tref can be calculated using a reference configuration q"f and a plurality of feedback loops 5 derived from measured sensor data. The control loop 404 may comprise a sixth filter 430, a configuration controller 432, a dynamic torque observer 434 and a gravity compensator 436. ;The control loop 404 may be a proportional derivative (PD) control unit, which is a type of controller whose output varies in proportion to the error signal as well as with the derivative of the error signal. ;The sixth filter 430 is connected to the position controller 432 and is configured to receive one or more incoming reference configuration values (such as from control unit 118). The sixth filter 430 is further configured to filter these values to provide as an input to configuration controller 432, q"f. The filter 430 is also configured to calculate a value for the first derivative with respect to time, coref, of this reference configuration (lief. Where the reference configuration is a reference position for the joint 406, the first derivative of this reference configuration (with respect to time) is reference velocity. The sixth filter 430 may correspond to the filters 418, 420, 422, 424, 426 of the joint controller 402. That is, the position filter may be configured to combine information from multiple data values to efficiently monitor the input position values, and to calculate a reference velocity value using a stored predefined sampling rate and the continuously sampled reference position values. ;The control loop further comprises a gravity compensator 436. The gravity compensator 436 is configured to receive reference configuration data from the sixth filter 430. A value gravity-compensated torque 1-0." can be calculated by the gravity compensator 436 using the reference configuration q"f rpm, can also be calculated using other position values, such as qvin described below. This gravity-compensated torque is provided to position controller 432. ;In addition to the gravity-compensated torque, the position controller 432 is configured to receive values of input configuration qt, output configuration qo and the first derivative of output configuration co, from the interconnect circuitry 428. The input configuration may be scaled to the same units as the output configuration, which may take account of a gear ratio of the transmission. ;The position controller 432 is also electrically connected to the sixth filter 430 and interconnect circuitry 428. The position controller 432 is therefore configured to, in this example, receive the input and output configurations (i.e. positions), and the first derivatives of these configurations, from the incoming interconnect circuitry 428. In other implementations, the position controller 432 may receive particular frequency components of these configuration signals and use these frequency components to determine the reference torque, as will be described in more detail below. The position controller is further configured to receive the reference configuration and first derivative of this value from the sixth filter 430. From these received values, the configuration controller 432 calculates a value for reference torque Tref to be provided to the joint controller 402. The reference torque Tref is transmitted from the command loop 404 to the joint controller 402 by outgoing interconnect circuitry 434. As with incoming interconnect circuitry 428, outgoing interconnect circuitry 434 may be a communication bus, such as an Ethercat bus. ;As introduced above, the output torque for a robotic joint can be characterised by the following equation: To = K(cli -90) where K is the spring constant of the joint, qi is the input configuration about the joint and q, is the output configuration about the joint. As previously mentioned, qi -q, represents the elongation of the drivetrain. qi and q, can both be measured using sensors coupled to the joint and its associated drivetrain. Thus, the output torque for the joint can be calculated using measured sensor data for the input and output joint configurations. This calculation of output torque can be used to replace a measured value of output torque obtained from a torque sensor. ;In one example, the configuration of the joint to be controlled by the control unit is the position of the joint. In this example, the reference configuration is the reference, or desired, physical position of the joint. The input and output configurations are the input and output positions of the joint respectively. ;In the examples described above, the input configuration qi that is used to calculate the reference torque is a measured configuration obtained from a sensor located at, or at a position proximal to, a first end of the drivetrain. In another example, input configuration may be calculated using a value of reference joint configuration that has been requested at a time prior to the time at which output joint configuration is measured. Ideally, for a "stiff' joint, a prior reference configuration will be the same as the input configuration of the joint. Thus, in this example, a sensor to measure input joint configuration is not required, increasing joint compactness. ;A PD controller on the input position sensor is stable based on passivity analysis. The joints of a robot arm may, in some implementations, not be stiff enough to be able to control the joint effectively using PD on the input due to the springiness of the joint. ;Using only the input position sensor signal, the system can be more robust, and using only the output position sensor signal, the system can be more accurate but less robust. Using a combination of the input position sensor signal and the output position sensor signal at certain frequencies, as described below, can allow the control scheme to be robust at high frequencies, where stability issues can emerge, and accurate at low frequencies, when good tracking is desirable. ;As mentioned above, it is a well known result that proportional derivative control using only the input position encoder results in a passive controller that will always be stable in connection with any passive, energy dissipating mechanical system. Such a controller can be represented as: Tref = 7,Tgrav Kp(Rref Ch) lc,i(Wref where (ire! is the reference position, coref is the reference angular velocity, co i is the input sensor angular velocity qi is the input position (scaled to the same units as the output position, which may take account of a gear ratio of the transmission), Trap is the gravitational torque term, e is an estimate of the joint efficiency, Kp is the proportional gain to the PD controller and Kru is the derivative gain being applied to the signal from the input position sensor. The angular velocity co is the derivative of the position signals q with respective to time. The estimated joint efficiency £ may be assumed to be 70%, i.e. 1 = 1.4 This control law would have improved stability properties, but in the presence of gravity and external loads, the elasticity of the joints may lead to unacceptable position tracking errors. ;Therefore, the following control law can be used, which uses the signal from the input position sensor at high frequencies (where most stability problems may occur) and the signal from the output position sensor at low frequencies (at which it is desirable to have low sensitivity to external force disturbances). ;A 'virtual' position signal can be defined, given by: qw,t = q0L + -where q, is the output position, qi is the input position and L is a low pass filter. Thus qwrt is calculated using the signal from the input position sensor at high frequencies and the signal from the output position sensor at low frequencies. In other words, the virtual position signal for the joint is calculated using frequency components of the input position signal that are within a high frequency range and frequency components of the output position signal that are within a low frequency range. ;qv can be used as a feedback term to calculate Tref * According to this control scheme, one example of the reference torque is given by: = 7 Tgrav Kp(gref qvirt)+ Kd,i(Wr where: qvirt = qoL + -and where q"f is the reference position, wref is the reference angular velocity, cot is the input sensor angular velocity, qo is the output position, qi is the input position, T -gray is the gravitational torque term, E is joint efficiency, Kp is the proportional gain to the PD controller, K is the derivative gain being applied to the signal from the input position sensor and L is a low pass filter. The angular velocity co is the derivative of the position signals q with respective to time.
As defined above, the 'virtual' position signal qvin represents the output position sensor signal at low frequencies and the input position sensor signal at high frequencies. An alternative notation is to re-write qvin as: (11717T (11 L (R0 ch) i.e. the input position sensor signal is used unless there is a persistent, low frequency difference between the input position sensor and output position sensor signals, in which case the input position sensor signal is corrected by the input-output mismatch.
Thus, in this control scheme, qw" is used as a feedback signal to the controller.
In another example according to this control scheme Tref may be given by: re f -Corot,
E )
where is the derivative of qvi" with respect to time.
The frequency components of q, that are within a low frequency range may be obtained by inputting q, to a low pass filter L. The frequency components of qi that are within a high frequency range may be obtained by inputting qi to a high pass filter (1-L).
The termKp a (-fret -quirt) is the difference between the reference position signal and the 'virtual' position signal, modified by a proportional gain. The term fci,i(co"f -coi) is the difference between the first derivative (with respect to time) of the reference position signal and the first derivative (with respect to time) of the input position signal qi, modified by a derivative gain being applied to the input position sensor signal.
The use of a torque term ro." which compensates for the action of gravity can prevent the drifting of the joint due to gravity during its operation.
In one example, a low pass filter may be used to obtain frequency components of the output position sensor signal that are predominantly above a frequency threshold, for example 3Hz. The frequency threshold may be, for example between 1-3 Hz. A low pass filter is a filter that passes signal components with a frequency lower than the selected frequency threshold and attenuates signal components with a frequency greater than the selected frequency threshold. An idealised low pass filter can be thought of as outputting frequency components of the joint configuration signal q, that are below a frequency threshold and an idealised high pass filter can be thought of as outputting frequency components of the drive source configuration signal qi that are above a frequency threshold. As is well know in the art, most real low pass filters do not have a step profile, so in reality, some of the frequencies passed by a low pass filter L and a high pass filter (1-L) may overlap (and in effect, the frequency thresholds are different for each filter).
The high frequency range may correspond to frequencies of greater than a first frequency value between 1 to 3 Hz. The low frequency range may correspond to frequencies of less than a second frequency value between 1 to 3 Hz. For example, the high frequency range may predominantly contain frequencies that are greater than a threshold of 3 Hz and the low frequency may predominantly contain frequencies that are less than the threshold of 3Hz. The high frequency range and the low frequency range may partially overlap. For example, the high and low frequency ranges may both contain frequencies that are within the range 2.5-3.5 Hz, due to common low pass and high pass filters not being step functions. The high frequency range may have a median (or other average) frequency that is higher than the low frequency range. The lowest frequency value within the low frequency range may be less than the lowest frequency value within the high frequency range. This may reflect frequency components of a signal that are output from a high pass filter (in the case of the high frequency range) and a low pass filter On the case of the low frequency range).
Figures 5 to 7 show examples where qvjrt is used as an input to the position controller to calculate In Figure 5, a position controller 500 receives a value of desired position goof. Sensors at a joint 501 measure the input position and the output position and signals g, and gi are formed. go and gi are sent to a virtual position sensor generator 502 and can also be used as feedback to the joint controller directly. At the virtual position sensor generator 502, the respective signals are input to respective filters (the input position sensor signal is input to a high pass filter and the output position sensor signal is input to a low pass filter) and CIvert which represents the high frequency range components -i of the input position sensor signal and the low frequency range components of the output position sensor signal, is passed to the position controller 500. At the position controller 500, Tref is determined, as discussed above, which is used by a joint controller to control the joint 501 In Figure 6, a position controller 600 receives a value of desired position g"f. Sensors at a joint 601 measure the input position and the output position and signals go and gi are formed. qo and qi are sent to a virtual position sensor generator 602 where overt is determined and passed to the position controller 600 to determine Tref.
In Figure 7, a position controller 700 receives a value of desired position q" f. Sensors at a joint 701 measure the input position and the output position and signals qo and qi are formed. q, and (11 are sent to a virtual position sensor generator 702 where q"" is determined and passed to the position controller 700 to determine Tref. At the position controller, a difference between civet and quirt is determined at 750. A PD controller 751 then calculates a value of Tref, for example using one of the equations for Tref above. The value of T is then used by a joint controller to control the joint It may also be beneficial to protect some joints of the arm, such as the wrist pitch and yaw joints of the robot arm, at high loads.
In some implementations, the control laws described above may be modified to protect the joint from excessive torques applied to the joint by yielding when the externally applied torque exceeds a certain value. For example: -qv 13) Ktctie-; + crni -where r, is the sensed torque at the joint output and is a static -here: As illustrated in Figure 5, for output torques less than T -max this additional feedback term is inactive and the controller behaves identically to the unmodified control law described above. However, when the output torque rises above 1-",," the additional C274- Tinex 0; -r max tmax
-I--
ntax5 Ti a 2: feedback term acts to reduce the motor torque command hence limit the build up of torque at the output of the joint. This approach may be more suitable for weaker joints of the robot arm. For example, this approach may be particularly applicable for the pitch and yaw joints of a wrist of the robot arm, because these joints are generally weaker than the more proximal joints of the robot arm and as a consequence may be more prone to damage during use.
A method for controlling a robotic joint using a control unit as described above is illustrated in figure 9.
A drive source configuration signal indicates a position of the drive source (such as qi above). This signal indicates the input position of the joint. The control unit may receive the drive source configuration signal from a first sensor, which may correspond to first sensor 412 in figure 4. As mentioned above with reference to figure 4, the first sensor 412 may be located at a first end of the drivetrain, at which the drive source 408 is located. The control unit may optionally receive a sensed torque signal, corresponding to the torque at the output of the joint, which may be used to calculate the additional torque term, as described above with reference to Figure 8.
A joint configuration signal indicates a position of the joint in the surgical robot (such as GIG above). The control unit may receive the joint configuration signal from a second sensor. This signal indicates the output position of the joint. The output configuration may be received from a second sensor, which may correspond to second sensor 414 in figure 4. In one example, the second sensor 414 may be located at the second end of the drivetrain at which the joint is located.
At step 901 of method 900, the control unit calculates a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range. The respective frequency components of the respective signals may be determined using one or more filters. For example, the frequency components of the drive source configuration signal that are within the high frequency range may be obtained by inputting the drive source configuration signal to a high pass filter and the frequency components of the joint configuration signal that are within the low frequency range may be obtained by inputting the joint configuration signal to a low pass filter.
At step 902, the control unit outputs the value of reference torque to a joint controller for controlling the joint.
The control method described above may receive a continuous stream of sensor data indicating the input and output configurations of the joint. Thus, the control algorithm may be configured to repeatedly calculate the value of reference torque r"f. The continuous calculation of reference torque ensures that the control system is able to instantaneously compensate for disturbances to the joint performance as they occur. In one example, this compensation may occur in substantially real time. This method can be used on all joints of a robot arm.
Figures 2 to 8, as described above, refer to the forces generated by joints as being torques. Thus, figures 2 to 8 refer to the controllers for controlling the configuration of a rotary joint. In an alternative example, the joint to be controlled may be configured to provide linear motion. In this example, corresponding forces can be measured and calculated using strain gauges or other force sensors.
The approach described herein may also improve robustness of the dynamic performance of the joint controller to mechanical variation. In other words, this may help to ensure consistent performance in the face of part-to-part variation and through-life variation of the system. The proposed control scheme is therefore more robust and more stable than existing control systems.
In particular, this control scheme can result in improved robustness, improvements in the high frequency tracking performance of the joint positions, and reduced current oscillations. The approach can also mitigate lack of smoothness of a joint and result in improved tolerance of low joint stiffness, as well as increased tolerance to variations in joint stiffness.
As a further benefit, the method described herein can in some implementations 5 remove dependence on torque sensors for position feedback, which leaves the torque sensors with the single function of detecting external loads. This simplifies specification, development and testing of torque sensors. The approach removes dependence on the torque sensors for position feedback by switching to using proportional-derivative control from qi at high frequencies, with proportional control 10 from qo at low frequencies. Thus, a control system using this method does not require a torque sensor (such as the third sensor 416 in figure 4) to measure the output torque about for the joint. The elimination of a sensor for measuring output torque can result in a more compact joint design, which in turn would reduce the number of assembly steps and the computational cost of the joint. The signal disturbances produced by noise within the system can also be reduced as a result of the reduction in sensor inputs.
Reference is now made to figure 10 which illustrates various components of an exemplary computing-based device 1000 which may be implemented as any form of a computing and/or electronic device, and in which embodiments of the methods and modification systems described herein may be implemented. The computing-based device 1000 comprises one or more processor 1002 which may be microprocessors, controllers or any other suitable type of processors for processing computer executable instructions. In some examples, for example where a system on a chip architecture is used, the processors 1002 may include one or more fixed function blocks (also referred to as accelerators) which implement a part of the method of modifying a data stream in hardware (rather than software or firmware). Platform software comprising an operating system 1004 or any other suitable platform software may be provided at the computing-based device to enable application software, such as software 1005 implementing the method of figure 9, to be executed on the device.
The computer executable instructions may be provided using any computer-readable media that is accessible by computing-based device 1000. Computer-readable media may include, for example, computer storage media such as memory 1006 and communications media. Computer storage media (i.e. non-transitory machine-readable media), such as memory 1006, includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Computer storage media includes, but is not limited to, RAM, ROM, EPROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disks (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information for access by a computing-based device. In contrast, communication media may embody computer readable instructions, data structures, program modules, or other data in a modulated data signal, such as a carrier wave, or other transport mechanism. As defined herein, computer storage media does not include communication media. Although the computer storage media (i.e. non-transitory machine-readable media, e.g. memory 1006) is shown within the computing-based device 1000 it will be appreciated that the storage may be distributed or located remotely and accessed via a network or other communication link (e.g. using communication interface 1008).
The computing-based device 1000 also comprises an input/output controller 1010 arranged to output display information to a display device 1012 which may be separate from or integral to the computing-based device 1000. The display information may provide a graphical user interface. The input/output controller 1010 is also arranged to receive and process input from one or more devices, such as a user input device 1014 (e.g. a mouse or a keyboard). This user input may be used to initiate verification. In an embodiment the display device 1012 may also act as the user input device 1014 if it is a touch sensitive display device. The input/output controller 1010 may also output data to devices other than the display device, e.g. a locally connected printing device (not shown).
In the description above actions taken by the system have been split into functional blocks or modules for ease of explanation. In practice, two or more of these blocks could be architecturally combined. The functions could also be split into different functional blocks.
The present techniques have been described in the context of surgical robotic systems, though at least some features described are not limited to such systems, but may be applied to robotic systems more generally. In some examples, the present techniques may be applied to robotic systems that operate remotely. Some examples of situations in which the present techniques may be useful include those that make use of 'snake-like' robots for exploration, investigation or repair. In the case of a surgical robot the end effector could be a surgical tool such as a scissors, surgical cutter, surgical pincer or cauteriser.
The applicant hereby discloses in isolation each individual feature described herein and any combination of two or more such features, to the extent that such features or combinations are capable of being carried out based on the present specification as a whole in the light of the common general knowledge of a person skilled in the art, irrespective of whether such features or combinations of features solve any problems disclosed herein, and without limitation to the scope of the claims. The applicant indicates that aspects of the present invention may consist of any such individual feature or combination of features. In view of the foregoing description it will be evident to a person skilled in the art that various modifications may be made within the scope of the invention.
Claims (20)
- CLAIMS1. A control unit for a surgical robot, the surgical robot comprising a joint driven by a drivetrain which transfers power from a drive source to the joint, wherein the control unit is configured to: in dependence on a drive source configuration signal indicating a position of the drive source and a joint configuration signal indicating a position of the joint in the surgical robot, calculate a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range; and output the value of reference torque to a joint controller for controlling the joint.
- 2. The control unit as claimed in claim 1, wherein the frequency components of the drive source configuration signal that are within the high frequency range are the output of a high pass filter taking the drive source configuration signal as input.
- 3. The control unit as claimed in claim 1 or claim 2, wherein the frequency components of the joint configuration signal that are within the low frequency range are the output of a low pass filter taking the joint configuration signal as input.
- 4. The control unit as claimed in any preceding claim, wherein the control unit is configured to calculate the value of reference torque using a position signal given by got, ±( 1 -a where q, is the joint configuration signal, qi is the drive source configuration signal and L is a low pass filter.
- 5. The control unit as claimed in any preceding claim, wherein the control unit is further configured to receive a reference input indicating a desired position of the joint.
- 6. The control unit as claimed in claim 5, wherein the value of reference torque is further calculated using the reference input indicating a desired position of the joint.
- 7. The control unit as claimed in claim 5 or claim 6, wherein the value of reference torque is calculated using a difference between a first derivative of the reference input and a first derivative of the drive source configuration signal.
- 8. The control unit as claimed in any of claims 5 to 7, wherein the value of reference torque is calculated using a difference between the reference input and a virtual position signal, wherein the virtual position signal is calculated using the frequency components of the drive source configuration signal that are within the high frequency range and frequency components of the joint configuration signal that are within the low frequency range.
- 9. The control unit as claimed in any of claims 5 to 8, wherein the value of the reference torque is given by the equation: Tref = I gray ± Kp(gre f -qv) f -where quirt = goL + g1(1-L) and where g"f is the reference input, co"f is a reference angular velocity, go is the joint configuration signal, gi is the drive source configuration signal, cot is the angular velocity for the drive source configuration signal, I gray is a gravitational torque term, E is joint efficiency, Kp is a proportional gain to the control unit,I Cd,i is a derivative gain being applied to the drive source configuration signal and L is a low pass filter.
- 10. The control unit as claimed in any preceding claim, wherein the drive source configuration signal is determined from data acquired by a first position sensor and the joint configuration signal is determined from data acquired by a second position sensor.
- 11 The control unit as claimed in claim 10, wherein the first position sensor is located at, or at a position proximal to, a first end of the drive train at which the drive source is located, and/or wherein the second position sensor is located at, or at a position proximal to, a second end of the drive train at which the joint is located.
- 12. The control unit as claimed in any preceding claim, wherein the drive source configuration signal is the measured angular position of the drive source, and the joint configuration signal is the measured angular position of the joint.
- 13. The control unit as claimed in any preceding claim, wherein calculating the value of reference torque comprises adding a torque term which compensates for the action of gravity on the joint.
- 14. The control unit as claimed in any preceding claim, wherein calculating the value of reference torque comprises adding a further torque term when the value of an externally applied torque at the joint exceeds a predetermined value.
- 15. The control unit as claimed in claim 14, wherein the value of the further torque term is determined in dependence on a function q5, where: ( TJ -Tmax, J intax < TJ < ;max.17 < -Tmax 0 (TJ; Tmax) = 0, -Tmax T j + ;mix, where 27 is the sensed torque at the joint output and wheremax is the predetermined 20 value.
- 16. The control unit as claimed in any preceding claim, wherein the high frequency range corresponds to frequencies of greater than a first frequency value between approximately 1 to 3 Hz and the low frequency range corresponds to frequencies of 25 less than a second frequency value between approximately 1 to 3 Hz.
- 17. The control unit as claimed in any preceding claim, wherein the control unit is configured to repeatedly calculate the value of reference torque.
- 18. The control unit as claimed in any preceding claim, wherein the drive source is a motor.
- 19. A method for controlling a surgical robot, the surgical robot comprising a joint being driven by a drivetrain which transfers power from a drive source to the joint, the method comprising: in dependence on a drive source configuration signal indicating a position of the drive source and a joint configuration signal indicating a position of the joint in the surgical robot, calculating a value of reference torque for the joint using frequency components of the drive source configuration signal that are within a high frequency range and frequency components of the joint configuration signal that are within a low frequency range; and outputting the value of reference torque to a joint controller for controlling the joint.
- 20. A computer-readable storage medium having stored thereon computer readable 15 instructions that when executed at a computer system comprising one or more processors cause the one or more processors to perform the method of claim 19
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
GB2219710.7A GB2625795A (en) | 2022-12-23 | 2022-12-23 | Joint control unit |
PCT/GB2023/053364 WO2024134218A1 (en) | 2022-12-23 | 2023-12-22 | Joint control unit |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
GB2219710.7A GB2625795A (en) | 2022-12-23 | 2022-12-23 | Joint control unit |
Publications (2)
Publication Number | Publication Date |
---|---|
GB202219710D0 GB202219710D0 (en) | 2023-02-08 |
GB2625795A true GB2625795A (en) | 2024-07-03 |
Family
ID=85130173
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
GB2219710.7A Pending GB2625795A (en) | 2022-12-23 | 2022-12-23 | Joint control unit |
Country Status (2)
Country | Link |
---|---|
GB (1) | GB2625795A (en) |
WO (1) | WO2024134218A1 (en) |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20220016769A1 (en) * | 2020-07-17 | 2022-01-20 | Seiko Epson Corporation | Overshoot Amount Detection Method And Robot System |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5642214B2 (en) * | 2013-02-15 | 2014-12-17 | 株式会社神戸製鋼所 | Elastic deformation compensation controller for articulated robot |
WO2020002473A1 (en) * | 2018-06-26 | 2020-01-02 | Subblue Robotics Aps | Robot arm, remotely operated vehicle and method of cleaning a sub-surface structure |
GB2596812B (en) * | 2020-07-06 | 2024-05-29 | Cmr Surgical Ltd | Joint control in a mechanical system |
-
2022
- 2022-12-23 GB GB2219710.7A patent/GB2625795A/en active Pending
-
2023
- 2023-12-22 WO PCT/GB2023/053364 patent/WO2024134218A1/en unknown
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20220016769A1 (en) * | 2020-07-17 | 2022-01-20 | Seiko Epson Corporation | Overshoot Amount Detection Method And Robot System |
Also Published As
Publication number | Publication date |
---|---|
GB202219710D0 (en) | 2023-02-08 |
WO2024134218A1 (en) | 2024-06-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11850014B2 (en) | Control system, control method, and surgical arm system | |
JP2022535728A (en) | Systems and apparatus for external torque observation and compensation of surgical robotic arms | |
JP2009537228A (en) | Method and apparatus for controlling a haptic device | |
US10272565B2 (en) | Motor, actuator, and medical support arm apparatus | |
WO2018051665A1 (en) | Medical support arm device, medical system, and surgical microscope system | |
Piqué et al. | Dynamic modeling of the da Vinci Research Kit arm for the estimation of interaction wrench | |
US20230320798A1 (en) | Joint control in a mechanical system | |
AU2020373346B2 (en) | Robotic joint control | |
GB2625795A (en) | Joint control unit | |
Zhu et al. | Adaptive control of harmonic drives | |
JP2020064576A (en) | Control system, control method, and control program | |
KR101302746B1 (en) | Method for measuring external force of robot | |
GB2621581A (en) | Controlling a joint in a robotic arm | |
Rahmani et al. | Application of a novel elimination algorithm with developed continuation method for nonlinear forward kinematics solution of modular hybrid manipulators | |
JP7439236B2 (en) | Characterizing the performance of robot joints | |
Parthiban et al. | A simplified approach to admittance-type haptic device impedance evaluation | |
JP2013526425A (en) | Method for identifying friction in a hinge of a robot arm or manipulator arm and use of the method in a torque compensation method | |
GB2623464A (en) | Robotic joint control | |
Sharma | A New Variable Stiffness Series Elastic Actuator for the Next Generation Collaborative Robot | |
Yılmaz | Development of a Sensorless Haptic Teleoperation System for Robotic Minimally Invasive Surgery | |
Mdeihly | Haptic Sensing in Teleoperated Surgery | |
Guiatni et al. | Modeling and instrumentation for position and torque control of a 4dof force-feedback device | |
JP2005078328A (en) | State feedback controlling device |