WO2024121565A1 - System and method for control of robot manipulators - Google Patents

Abstract

Systems and methods for synthesising a controller of a robotic manipulator system are described, whereby a model of the robotic manipulator system is provided, the model including a plurality of control parameters associated with one or more virtual control mechanisms attached to respective locations of the robotic manipulator system, wherein output control actions of the model are defined based on said plurality of control parameters of the one or more virtual control mechanisms. Linear matrix inequality, LMI, constraints are determined for synthesis of control parameters that achieve a defined performance, wherein the LMI constraints define a gain matrix, K, consisting a plurality of diagonal matrices that correspond to the plurality of control parameters. A solution to the LMI constraints is derived, wherein the solution identifies control parameter values in the plurality of diagonal matrices. Output control actions are determined for one or more actuators of the robotic manipulator system based on the identified control parameter values.

Description

1 System and Method for Control of Robot Manipulators Field of the Invention [0001] This invention relates generally to a robotics system, and more particularly to control systems for robotic manipulators and the design of such systems. Background [0002] There are many considerations when designing a controller for a robotic manipulator or articulated arm, covering a wide range of tasks such as pick-and-place operations, cooperative robotics, teleoperated robots and robotic surgery. A successful control approach may typically provide stability and remain stable when interacting with the environment. A good approach may also shape the robot's performance. However, doing so analytically is made difficult by the nonlinear nature of the dynamics of robot manipulators. [0003] Passivity-based control (PBC) is a powerful technique widely used in control design for robotics. PBC provides strong stability guarantees, via straightforward energy considerations. The most widely used controller in robotics, the joint-space proportional-derivative (PD) control with gravity compensation, is a simple and robust passivity-based controller. Its energy shaping and damping injection features guarantee passivity of the controlled robot. Passivity-based control design techniques have also been adopted for parameter estimation, remote-teleoperation, and trajectory tracking via passive-aware adaptive estimation of the parameters of the system. [0004] Conventional impedance control schemes typically implement passive impedance defined by a desired stiffness, damping, and inertia, implemented by torque/force controlled joints. An impedance controller implementing a desired stiffness and damping without modifying the inertia (which is practically difficult), is a form of PBC energy-shaping and damping injection. Thus, it shares the same stability properties. At the same time, impedance control opens the way to the characterization and control of the robot performances, via loop shaping and similar considerations. [0005] However, in practice, selection of desired impedances and determining optimal control parameters is a challenging task. Some existing approaches for selecting impedances involve machine learning for variable impedance control by varying the stiffness, but passivity of the controller is lost. Methods to preserve passivity exist, such as tank-based variable impedance control, but using this approach requires a careful consideration to ensure that “energy-tanks” do not deplete, hindering performance. [0006] There is a need to establish and implement improved methods and systems for systematic specification and optimisation of impedance control parameters in a robust control system. Summary of the Invention [0007] Aspects of the present invention are set out in the accompanying claims. [0008] According to one aspect, there is described a method to synthesise a controller of a robotic manipulator system, the method comprising: providing a model of the robotic manipulator system including a plurality of control parameters associated with one or more virtual control mechanisms attached to respective locations of the robotic manipulator system, wherein output control actions of the model are defined based on said plurality of control parameters of the one or more virtual control mechanisms; determining linear matrix inequality, LMI, constraints for synthesis of control parameters that achieve a predefined performance, for example as measured local ^gain between selected force/displacement variables, wherein the LMI constraints define a gain matrix, K comprising a plurality of diagonal matrices that correspond to

the plurality of control parameters; deriving a solution to the LMI constraints, wherein the solution identifies control parameter values in the plurality of diagonal matrices; and determining output control actions for one or more actuators of the robotic manipulator system based on the identified control parameter values. Typically, the identified control parameter values may be optimal values. [0009] The virtual control mechanisms may comprise one or more of virtual springs and dampers attached to respective points of the robotic manipulator system. The gain matrix may have the form where K defines virtual stiffness control

parameters and defines virtual damping control parameters of the virtual spring-

damper control mechanisms. Generalised plants may map input exogenous forces (

and control actions

to output respective exogenous displacements (z_d) and measured observation outputs ǡ of the manipulator system. A feedback control loop may

optimize performances on a performance pair defined by The predefined

performance metric may be an

control criterion. [0010] Advantageously, the controller is configured to preserve passivity of the robot and to maintain stability of the end-effector by deriving a solution that forces the values of the control parameters matrices to be diagonal and positive. The model may be linearized in operation-space in the process of optimizing the control parameters. [0011] The number of virtual control stiffness/damping parameters may be less than or equal to the degrees of freedom of the robotic manipulator system. Actuators of the robotic manipulator system may be excited based on the output control actions. [0012] The virtual control mechanisms may further comprise a virtual dynamic extension mechanism having a prismatic joint component to provide extension control along a defined axis of the robotic manipulator system. The defined axis may be a common axis of an instrument attached to the robotic manipulator system. A first pair of virtual spring-damper control mechanisms may be attached to an end-effector of the instrument, and a second pair of virtual spring-damper control mechanisms are attached to the virtual dynamic extension mechanism. The second pair of virtual spring-damper control mechanisms may be rotated about the prismatic joint component of the virtual dynamic extension mechanism to align the extension control with a defined direction. [0013] Alternatively, the virtual control mechanism may further comprise a dynamic extension mechanism having a virtual instrument attached to a physical instrument by a plurality of virtual spring-damper control mechanisms. A further pair of virtual spring- damper control mechanisms are attached to an end-effector of the physical instrument. Each virtual spring-damper control mechanism may comprise a co-located spring and damper component. [0014] In other aspects, there are described systems configured to perform the methods as described above. In a further aspect, a computer program is described, comprising machine readable instructions arranged to cause a programmable device to carry out the described methods. The instructions may be stored in non-transient form. Brief Description of the Drawings [0015] There now follows, by way of example only, a detailed description of embodiments of the present invention, with references to the figures identified below. [0016] Figure 1 is a block diagram showing the main components of a robot control system according to an embodiment of the invention. [0017] Figure 2 schematically illustrates an exemplary robotic manipulator system in the control system of Figure 1. [0018] Figure 3 is a flow diagram illustrating the main processing steps performed by the control system of Figure 1 to synthesise a controller of the robotic manipulator system according to an embodiment. [0019] Figure 4, which comprises Figures 4A and 4B, schematically illustrates an example of a robot system under PD control in joint-space compared to operation-space. [0020] Figure 5 is a block diagram schematically illustrating the overall structure of an exemplary implementation of the controller of Figure 1 according to an embodiment. [0021] Figure 6 schematically illustrates a sectional view of an exemplary robot for robotic minimally invasive laparoscopy. [0022] Figure 7 schematically illustrates a robotic manipulator system with a virtual prismatic extension mechanism according to a further embodiment. [0023] Figure 8, which comprises Figures 8A and 8B, schematically illustrate a modification to the prismatic extension of Figure 7. [0024] Figure 9 schematically illustrates a robotic manipulator system with a virtual instrument mechanism according to another embodiment. [0025] Figure 10 is a block diagram schematically illustrating the overall structure of a virtual mechanism controller based on the systems shown in Figures 7 and 9. [0026] Figure 11 graphically illustrates the control performance of a first simulated example. [0027] Figure 12 shows a redundant arm of a second simulated example. [0028] Figure 13 graphically illustrates the control performance of the second simulated example. [0029] Figure 14, which comprises Figures 14A and 14B, shows a robot arm of a third simulated example. [0030] Figure 15, which comprises Figures 15A to 15C, graphically illustrates the control performance of the third simulated example. [0031] Figure 16 is a diagram of an example of a computer system on which one or more of the functions of the embodiment may be implemented. Description of Embodiments [0032] Generally, the present invention provides a method and system for torque/force control of robotic actuators, such as linear or rotary hydraulic actuators of a rigid robot. The impedance selection/optimization problem is re-cast into a robust control problem, taking advantage of the robot linearization in operation space. The control system may implement potential and/or kinetic energy shaping and damping injection with optimized performance, by synthesizing the interconnection of the robot with a system of virtual control mechanisms. Working in operation space, the controller enables intuitive considerations on the shaping of the robot potential energy. [0033] Figure 1 is a block flow diagram schematically illustrating a robot control system 101 according to an embodiment, including a controller 103 to control a robotic manipulator system 105 via control actions 107 determined by a dynamic model 111 of the manipulator. Reference is also made to Figure 2 schematically illustrating an exemplary implementation of a robotic manipulator system 105a according to the present embodiment. The controller 103 for the robotic manipulator system 105 is configured to act on the generalised force of the robot, u, via feedback from generalised coordinates

. The control actions

107 are generated by emulating a plurality of virtual control mechanisms 109 attached to respective points of the robotic manipulator system 105. Each virtual control mechanism 109 includes one or more virtual control elements 209 that may be combined to enable straightforward tuning of the energy landscape for a particular implementation context while preserving passivity of the robot, for stable interactions with the environment. The virtual control elements 209 may include energy storage elements such as springs 209a, inerters 209c, masses or inertias, energy dissipation elements such as dampers 209b, and energy routing elements such as rigid-body joints, levers, and mechanical transmissions. Advantageously, the virtual control mechanisms 109 embody classical passivity properties at co-located force/velocity or torque/angular velocity ports. [0034] As shown in the example of Figure 2, the robotic manipulator system 105 includes an end-effector 201 such as a tool to handle a payload or perform a task. The end-effector 201 is provided at the distal end of a sequence of rigid limbs (also referred to as links or segments of an articulated arm) 202, with respective force/torque actuators 203 provided at the joints between limbs 202, that are operable to move the end-effector 201. The placement of each virtual control mechanism 109 may be dictated by the features of the task and by considerations on the robotic manipulator's dynamics. The exemplary robotic manipulator system 105a of Figure 2 includes two virtual Cartesian spring-damper control mechanisms 109a-1,109a-2 attached to a predefined location at the end-effector 201. The virtual spring-damper control mechanisms 109a effectively provide Cartesian impedance control as a mechanism of Cartesian springs 209a and dampers 309b attached between the end-effector 201 and a reference position

. The respective location of the, or each, virtual control mechanism 109 (and/or components of each mechanism) may be stored as robot dynamic model data 111, for example by a constructor module 113 of the system 101. [0035] The constructor module 113 implements functionality to construct the controller 103 to achieve a defined task, such as moving the end-effector 201 to the desired position/orientation

( ) In general, the robot control system 101 may be defined in terms of a general control problem formulation, where the generalised plant models ( ) 115 map input exogenous forces (

117 and control actions 107 to output

exogenous displacements/positions (

) 119 and measured observation outputs

121, by linearising in the controller operation-space at a set of respective poses (q_i). The observation outputs ^{^} ^ ^{^} 117 are inputs to the constructed controller ( 103, while the

control actions 107 are output by the controller 103. Force/torque actuators 203 of

the robotic manipulator system 105 may be excited based on the control actions 107. The measured observation outputs 117, such as positions and velocities may be

measured by appropriate sensors (not shown) and/or calculated by the controller 103. The dynamic model 111 may be a software-based model created with simulation modelling tools such as MATLAB. [0036] The robot dynamic model 111 is configured to model the dynamical behaviour of the robot manipulator system, based upon its kinematic structure and physical properties, as well as its interconnection with virtual control mechanisms 109 and exogenous inputs and outputs. Each virtual control mechanism 109 may be associated with one or more respective types of control parameters 123 corresponding to the virtual elements 309, such as a stiffness (k) control parameter 123-1 of a virtual spring element 309a, a damping (b) 123-2 control parameter of a virtual damping element 309b, and an inertance (m) control parameter 123-m of a virtual inerter 309c. The controller 103 determines control actions 107 based on the respective control parameters 123 of the virtual control mechanisms 109. The constructed controller 103 may be defined by values of a gain matrix (K). For example, the gain matrix values may represent the respective stiffness (k) 123-1 and damping (b) 123-2 control parameters of each virtual spring-damper control mechanism 109a as shown in Figure 2. The gain matrix may consist of a plurality of block diagonal matrices corresponding to the plurality of types of control parameters 123. [0037] The virtual control mechanisms 109 implement operation-space coordinates to facilitate simple definition of where the components are attached about the robotic manipulator system 105. As will be described in greater detail below, the interconnection of the virtual control mechanisms 109 with the robotic manipulator system 105 is achieved by a transformation from the joint-space coordinates of the robot 105 to various ‘operation-space’ coordinates, for example by a Jacobian transformer module 125 of the controller 103 converting efforts and flows between operation-space and joint-space. An operation-space coordinate may be defined as the velocity of a point which is fixed relative to a link of the robot, such as the velocity of the end-effector 201. The corresponding port for this operation-space coordinate may be defined as the flow/effort pair of

where ^ is the velocity 127 of the end-effector 201 in the

world-frame, and

is a force vector 129 in the same (world) frame as the velocity, such that

is the power flow into the system 101. The operation space variables may be defined according to other suitable interpretations, such the link-frame velocity of a fixed point in the world, or the relative velocity of two different points on the robot 105. In such a case, the virtual control mechanisms 109 may be defined to be aligned with link-axes rather the world’s xyz axes, and/or attached between two points. [0038] In the present embodiment, the system 101 implements a potential and/or kinetic energy shaping and damping injection controller 103, defined by the robot dynamic model 111 and corresponding control law. Energy shaping and damping injection methods are generally known in robotics and the control of electromechanical systems. The core idea is that a passive system may store energy, route it between ports or dissipate it, but it may not produce energy. When two passive systems are connected together by compatible ports, then the resulting combined system is passive. The shaping of the energy function changes the equilibria of the system, a feature that can be used to control the robot to a desired position. The damping removes energy, affecting the speed of convergence. In general, the design of an energy shaping and damping injection controller to stabilize a nonlinear system at a particular equilibrium involves solving a partial differential equation. [0039] Accordingly, the constructor 113 is configured to determine optimal values of the control parameters 123 that will directly affect the performance of the system 101 based on the predefined placement of the corresponding virtual control mechanisms 109. The controller design method implemented by the constructor 113 is based on Linear Matrix Inequality (LMI) constraints to automatically synthesize optimal control parameters 123 of the controller 103where the synthesis is driven by gain criteria

from exogenous input (^_ௗ) to exogenous outputs (

[0040] As will be described in greater detail below, an LMI determiner 131 of the constructor 113 is configured to determine an LMI problem formulation, i.e. a convex optimization problem with linear matrix inequality constraints, based on the virtual control mechanisms 109 and the robot dynamic model 111. For example, the LMI determiner module 131 may convert the set of generalised plants ( 115 into LMI

constraints which may determine optimal values of the corresponding control parameters 123. The LMI solver 133 could be of a type that is generally known in the art (e.g. MATLAB packages such as CVX). In this way, a static-state feedback synthesis can be performed to systematically select stiffness and damping control parameters 123-k and 123-b. [0041] In the present embodiment, the system 101 is configured to force the output feedback gains of each type of control parameter 123-1,123-2 to be diagonal and positive. For example, the constructor 113 may determine an LMI problem formulation of the robot 105a in Figure 2 that produces a solution having stiffness (k) and damping (b) parameter matrices 123-1,123-2 that are diagonal and positive. Advantageously, this provides improved simplicity and effectiveness, and enables a systematic approach to selecting impedance parameters for a common class of impedance controllers, based on criteria of optimality with respect to

metrics. [0042] In this way, the structure of the virtual control mechanisms 109 provide a set of basis-functions/primitives that can be used to design an Euler-Lagrange sub-system that, when connected in feedback with the robot 105, achieves closed-loop passivity. The closed loop is passive at any port, whereby local minima of its potential energy are all stable (and asymptotically stable when damping is present). These features are preserved even under interaction with the environment, so long as the environment is passive. Furthermore, the controller implementation is simple as no inverse kinematics required, while providing design flexibility to handle multiple simultaneous objectives, such as end-effector and/or Remote Centre of Motion (RCM) control. Finally, the virtual control mechanisms are an excellent tool to facilitate production of controllers that are inherently more interpretable by users, thereby facilitating better communication between practitioners and more effective debugging by control engineers. Energy Shaping and Damping Injection Controller [0043] Figure 3 is a flow diagram of a computer-implemented method to synthesise a controller 103 of a robotic manipulator system 105, according to an embodiment. At step S3-1, the constructor module 113 defines a controller 103 of the system 105 based on the robot structure 111. As discussed above, the locations of the virtual control mechanisms 109 may be predefined in the robot structure 111 based on the features of the task and by considerations on the robot’s dynamics. [0044] Referring to the example shown in Figure 2, each virtual spring-damper control mechanism 109a includes a spring element 209a and a damper element 209b. Virtual springs 209a are used to drive the robot to the desired position, determining the equilibria of the system 105. The characteristic of a linear spring

, where z refers to the spring’s extension and

the spring stiffness, shapes the potential energy of the system 105 with a quadratic potential ²

. Likewise, nonlinear springs 209a with more complex characteristic functions

are equally simple to implement. Different effects such as deadzones or saturations can thus be achieved. The energy stored in the spring should be bounded below, for example by defining a potential function for the nonlinear spring and a finite constant C such that

( ) for all z,

where is the energy stored in the nonlinear spring, such that

Otherwise, an infinite amount of energy could be extracted from the spring 209a. [0045] Damper elements 209b are another vital component of a passive robot controller 103. Without damping, the system 105 may only be marginally stable, and simply oscillate, exchanging potential/kinetic energy like a swinging pendulum. Although robots, as mechanical systems, generally have some inherent damping from friction, the injection of additional damping by damper elements 209b advantageously provides for improved performance shaping, as damping affects how the system 105 approaches equilibrium. An ideal linear damper produces a force

^ where c is the damping coefficient of the damper and must be positive, and

^ is the velocity across the damper 209b. A linear damper 209b is passive so long as Nonlinear dampers F =

damper can only remove energy from the system. A nonlinear damper may be parametrised as a linear damper with a varying but always positive damping coefficient

[0046] Figure 4A schematically illustrates an example of a robot system 105a under PD control in joint-space. As shown, the exemplary robot system 405 includes two virtual spring-damper control mechanisms 109a-3,109a-4, with corresponding desired joint angles ^. The robot system 405 may be represented by the robot model:

where

^ represents Coriolis and centrifugal forces,

represents gravity forces, and խ

is an n x n joint-space kinetic energy matrix, based on the observation outputs of the robot system. u are the control outputs to the joint actuators 203 based on the dynamic model 111, with total energy:

where is the generalised inertia matrix and

the gravitational

potential energy. The gravity force/torque vector may be obtained by taking the partial derivative of the gravitational potential energy:

[0047] The energy function may be shaped by selecting:

where the elements of the vector k define the virtual stiffness parameters 123-1 and are all strictly positive. This can be achieved with a controller having the form:

where the vector

defines the virtual damping parameters 123-2 and are also strictly positive elements. The term ensures dissipation and guarantees that the

system trajectories converge asymptotically to the minimum of this energy function at

[0048] Moving away from joint space allows for advanced design of the robot’s response to external perturbations and avoids the computation of _թ via inverse

kinematics. In this way, the energy of the robot can be shaped in a space that is closer to the control objective. For position control of the end-effector 201 of a planar manipulator 105, for example, the robot model can be defined with the pair of virtual spring-damper control mechanisms 109a-1,109a-2 on respective Cartesian axes between the end-effector 201 position, z_c, and the reference position, r.This is shown in Figure 4B, which schematically illustrates the exemplary robot system 105a of Figure 2 under PD control in operation-space, instead of joint-space as shown in the example of Figure 4A. [0049] The present embodiment implements the operation-space configuration, and using the forward kinematics the corresponding desired energy function may

be defined as:

This can be achieved with a controller model having the form:

and a control law of the form:

where

[0050] The control action u emulates the effect of the force F

at the end-effector 201, realizing a proportional and derivative control action in the coordinates of the end- effector 201. This approach offers several advantages. The potential energy is a function of end-effector displacement 119. This implies that the response of the robot to external perturbations is independent of the robot’s configuration. Furthermore, there is no need to implement an inverse kinematics to drive the robot to a desired position This is

particularly important for redundant robots, where several configurations can attain the same end-effector position. In fact, for redundant robots, the controller 103 is operable to control the end-effector 201 position without forcing high-stiffness on each joint as in the case of a joint-space model. Parameter Tuning and Impedance Control [0051] The control parameters 123 of the controller 103, such as the stiffness and

damping (

parameters of the respective virtual control mechanisms 109, shape the relationship between

following the approach of impedance control. The controller constructor 121 implements a predefined metric to measure the performance of the robot and the criteria to optimize such performance, in order to establish a systematic framework for the selection of the control parameters 123. [0052] In the present exemplary implementation, performance of the robot system 105 is based on the performance pair

measured via the metric. The performance

pair

captures the effect that a perturbing exogenous force

has on the displacement

High performance position control, for example, may be provided by insensitivity of the displacement

to the perturbing force

This corresponds to enforcing a low gain between

In contrast, a compliant robot would allow for large displacements

for small perturbations F

In this setting, the selection of the parameters k and b can be guided by optimization. [0053] As schematically illustrated in Figure 1, the system 101 implements a feedback control loop to optimise performances on

The loop is closed, by the controller 103, on a control pair based on the observation outputs ^{^} 121 and the control inputs

107, with the goal of shaping the behaviour captured by the performance pair

This setting finds contacts with impedance control and classical robust control synthesis. Indeed, impedance control recognizes that positions and forces cannot be controlled independently and accordingly, the controller 103 is designed to shape the relationship between these quantities. Likewise, in the linear setting, the constructor 113 implements robust control to synthesise a controller 103 that minimize the gain between . In this way, the controller 103 of the present embodiment interconnects the

robotic manipulator 105 and virtual control mechanisms 109 by implementing operation-space impedance control and building a connection between closed-loop impedance shaping and robust control synthesis, and also implements a systematic framework for the determination of control parameters taking into account defined performance considerations. [0054] Figure 5 is a block diagram schematically illustrating the overall structure of an exemplary nonlinear controller 103a according to the present embodiment. The controller 103a implements conditions based on linear matrix inequalities that lead to control parameters 123, such as the stiffness (k) and damping (b) parameters, that preserve the passivity of the robot manipulator 105. As shown, the robot dynamic model 111 discussed above with reference to Figure 4B is further adapted with auxiliary input/output variables, as follows:

where is the Jacobian corresponding to the directions of applied disturbance force

As discussed above, ^ ^ represents Coriolis and centrifugal forces,

) represents

gravity forces, and

joint-space kinetic energy matrix, based on observation outputs

^ of the robot system. [0055] In the present exemplary implementation, a control pair defined by

is used to design the controller 103a having the form set out by equations (2) and (3) above in equations (2), (3) is based on the locations of the virtual control

mechanisms 109 as predefined by the robot structure. This corresponds to the particular shaping of the desired energy given by equation (1). It is also assumed that h_c is a differentiable invertible function within the region of interest. This will allow the system 101 to derive an equivalent representation of the robotic manipulator 105 in the coordinates z_c, which suits linearization methods for state-feedback design. [0056] Optimization of weighted performances may be represented by

where W are constant, diagonal matrices whose role is to scale, non-uniformly,

the output exogenous displacement components of

and its derivative Ī , respectively.

[0057] Advantageously, this implementation connects the energy shaping and damping injection approach with impedance control

synthesis of stiffness and damping controller parameters 123-1,123-2 to optimise performance of the robot manipulator 105. Robot Linearization in Operation Space [0058] For the systematic synthesis of stiffness and damping parameters vectors k and b 123-1,123-2 to optimise local performances, the controller 103a of the present embodiment is adapted to implement a state-feedback gain matrix where K₁ is a first block diagonal matrix (diag(k)) defining values of the virtual stiffness control parameters 123-1, and K₂ is a second block diagonal matrix

defining values of the virtual damping control parameters 123-2. Additionally, all the elements of and

must be strictly positive, to guarantee that the control action 107 defined by equations (2) and (3) leads to a passive controlled robot manipulator 105. [0059] To achieve this, the constructor 113 first derives the robot linearization in operation-space, then determines the state feedback gain matrix K values via LMIs, which will lead to the control law based on equation (3) having the form:

where F_c represents an operational force vector resulting from the optimised control parameters in combination with the dynamics of the plant 111. [0060] Accordingly, returning to Figure 3, at step S3-3, the LMI determiner 131 derives a state-feedback controller 103 by converting the robot model from step S3-1 into a linearised form in operative space. For example, the exemplary controller 103a of Figure 5 implements the energy shaping and damping injection structure of the dynamic model 111 as discussed above, whereby state-feedback gains can be synthesised with guaranteed performances. Representing the dynamics in operation-space corresponds to making a different choice of general coordinates when deriving the robot dynamics. As a result, the operation-space dynamics are defined by a dimension N, the number of degrees of freedom of the robot manipulator 105. In practice, it is difficult to derive these dynamics directly from Lagrangian methods for arbitrary operation-space variables. The exemplary implementation therefore draws an equivalence between the terms of the joint-space dynamics, M(q), ^^^ǡ ^^ ^ and g(q), and their operation-space equivalents. Differentiating Ī_c gives the acceleration

[0061] Combining this with the control law set out in equation (2) and the dynamics set out in equation (4), assuming a non-singular and therefore invertible Jacobian gives the operation-space dynamics:

[0062] For completeness, arguments q and q^լ can be written as functions of

, by the inverse kinematics

and operate in the region of invertibility of

However, in practice it may be preferable to keep the arguments in joint variables. The inverse kinematics are non-unique, so characterising and finding inverse kinematics functions is in general hard. In the present exemplary implementation, this is avoided by computing ¨ directly from a pose , without explicitly defining an inverse kinematics

so long as the forward kinematic is satisfied and z h ( )

[0063] By implicitly linearizing at a predefined equilibrium defined at

Ī

and using

^to denote small displacement variables, the following linearized operation-space dynamics is obtained:

[0064] Using the state variable Ȃ Ă and the output variable [^

from equations (5) and (6) leads to the linearized state-space system

with matrices

Synthesis For Energy Shaping And Damping Injection [0065] At step S3-5, the controller 103 solves the determined LMI problem formulation, for example using the LMI solver 133, to determine a solution defining synthesised state-feedback gains with guaranteed performances, the gains represented by values of the control parameters 123 corresponding to the virtual spring-damper control mechanisms 109 that optimise the predefined performance metrics on In

the present exemplary embodiment, synthesis of the virtual stiffness control parameters 123-1 and the virtual damping control parameters 123-2 is performed based on

synthesis. Specifically, the controller 103a determines and locally optimises the performances captured by the performance pair

measured via the metric. [0066] The determination of the control parameters 123 to

optimize local performances is derived via LMI constraints. The system 101 implements LMI to solve the below problem formulation for sparse synthesis and passivity, with additional

constraints to ensure that are positive definite diagonal matrices. Consider a

selection of poses the associated linearization given by equation (7), and the

matrix

where the subscript

is used to denote the matrices associated with the linearization

are matrices of opportune dimensions, and Ȗ is a scalar representing the

level of performance, with smaller Ȗ corresponding to tighter position control (higher stiffness of the robot). In the present embodiment, the LMI problem formulation is defined to: find such that

[0067] This is a modified LMI formulation based on the established LMI formulation as discussed, for example, in Section 7 of Linear Matrix Inequalities in System and Control Theory, Boyd et al., Society for Industrial and Applied Mathematics, January 1994, which implements the repeated use of matrix (8) for each linearization

. Any solution to the modified LMI problem returns a uniform state-feedback controller (7c) given by ¹

that guarantee passivity and local performances in terms of

gain less than Ȗ > 0 from the input F_d to the output y, for each linearization . The LMI

problem may be efficiently solved by convex optimization, for example using the LMI solver 133. Feasibility of this LMI problem for small Ȗ guarantees that exogenous

perturbing forces

induce small perturbations (in the

^sense) on positions and velocities of the robot, locally around each equilibrium

. A small Ȗ can provide good

performance on the controlled position locally near each equilibrium. [0068] At step S3-7, the controller 103a determines the output control actions (u,u_c) 107 based on the optimal control parameters 123 of the dynamic model 111 of the constructed controller 103a, for example as solved by the LMI solver 133 according to the performance criterion. The inventors have proven that the control action ( ) 107

obtained from any solution to the modified LMI problem guarantees passivity of the controlled robot at any port where is a vector of forces (or torques) and ^ is

the corresponding vector of co-located velocities (or angular velocities). This is crucial for stable interaction. At the same time, the controller 103a guarantees local performances, as long as the perturbing force is small enough to not invalidate the

linear approximation set out by equation (7). This is clarified by the following Theorem and Proof. [0069] Lemma: For any solution to the modified LMI problem of the present embodiment, take

Then, are diagonal positive definite matrices.

Theorem: Let

be the selection of poses with associated linearization in the

modified LMI problem. For any solution to the modified LMI problem, take k and b in equation (3) such that [diag( ^{í^}

Then, the controlled robot defined by equations (2), (3) and (4) is passive at any port and satisfies the local gain:

for the initial condition and reference for any sufficiently

small

Proofs [0070] Proof of Lemma: Define

Then,

(10) Note also that, by block inversion,

where

[0071] With these definitions we can now prove that

are diagonal matrices. Recall that if a matrix is diagonal, it remains so after inversion and transposition. Furthermore, product and sum of diagonal matrices lead to diagonal matrices. Since P = this implies that are diagonal matrices. Thus, from (10),

are also diagonal matrices. [0072] We can also prove that

are positive definite. For instance, recall that therefore Furthermore,

recall that inversion preserve positivity, and that the product and the sum of two positive diagonal matrices are positive and diagonal. Thus, using again P

, we have

where we have used the assumption

in the modified LMI problem set out above. Finally, using (

from the modified LMI problem, we can conclude that

are positive definite. [0073] Proof of Theorem: We first prove passivity. Consider the controller defined by equations (2) and (3). By the above Lemma, diag(k) and diag(b) are positive definite matrices. For the robot equation extended with a generic additional port

where consider the energy function as stated in (1). This is a well

defined energy function since diag(

Then,

where the inequality follows from diag(b) ظ 0. This proves passivity. [0074] We now show the local gain inequality. Recall that

and consider the local storage function

where P is defined in (9). Then, (8) implies that there exists some (small) İ > 0 such that as long as

this implies that

[_{0075] Thus, if the perturbation}

_{d is small enough to keep}

for all , we can extend the interval of integration to

ĺ , arriving at

Virtual Dynamic Extension Mechanisms [0076] In the field of robotic minimally invasive surgery, the robot may enter a patient's body through a trocar, a surgical device used to create and maintain a portal through the skin for the insertion of a physical, e.g. surgical, instrument. Figure 6 schematically illustrates a sectional view of an exemplary surgical robot 105 for robotic minimally invasive laparoscopy, where the end-effector 201 of a surgical instrument 601 enters a patient's body 603 through a trocar 605. The robot’s end-effector 201 may follow a surgeon’s motion, for example as captured by the surgeon's inputs via a console. The surgeon may orchestrate a plurality of surgical instruments and an endoscope (a minimally invasive camera) simultaneously. Different types of robotic minimally invasive surgery pose different technical challenges. For example, in thoracic surgery, which takes place in the chest, the trocar 605 may intrude upon the ribcage. In arthroscopy (keyhole surgery of a joint such as a hip or knee), the workspace is typically extremely limited by the anatomy of the joints, and the trocar 605 may be located much closer to the site of the operation compared to abdominal laparoscopy, for example. [0077] The surgeon may designate a desired position and/or attitude of the end- effector 201 using a user interface of the console. The system 101 may sample that position and/or attitude from time to time and use the sampled position/attitude as an input to the algorithm described herein. Typically, the surgeon exclusively controls the motion of the end-effector 201 and the robot’s controller 103 ensures that the instrument 601 passes through a portal site, for example through the trocar 605 or through an incision not protected by a trocar. The position of the trocar 605 and an associated Remote Center of Motion (RCM) 607, is typically recorded at the beginning of the procedure, denoted and used to constrain the motion of the instrument 601 such that the RCM 607 passes through the trocar 605. However, in some procedures it may be advantageous to represent the trocar task in a less simple way. For example in thoracoscopic surgery, the chest wall may restrict movement, which can result in trauma at the incision site or nerve damage if the robot and controller can not or does not account for the restrictions, resulting in undesired forces applied to the port site. Conversely, it can be beneficial to relax the RCM constraint to improve mobility of the end-effector through the port site (e.g. by expanding the RCM constraint from a point to a hoop), for example in trans-oral surgery, arthroscopy, small volume-laparoscopy. [0078] Accordingly, further embodiments will now be described of a robot controller that emulates virtual control mechanisms to enable highly flexible, efficient and accurate control of an end-effector through a port site. Corresponding reference numerals to those of preceding figures are used where appropriate for corresponding elements in the further embodiments. Prismatic Extension [0079] Figure 7 schematically illustrates a robotic manipulator system 105b with an exemplary implementation of a virtual dynamic extension mechanism according to a further embodiment. As shown, the robot 105b may be a surgical robot comprising an articulated arm with a distal segment 202-3 having an attachment for a surgical instrument 601. The robot 105b is adapted to provide improved control of the instrument 601 through a port site defined by a trocar 605 for example, by further allowing sliding motion of the end-effector 201 along the longitudinal axis of the instrument 601. [0080] To enable the sliding motion, the system 105b includes a virtual prismatic extension control mechanism 109b-1, having a virtual prismatic joint 209c that can slide along the longitudinal axis of the instrument 601, and a virtual inerter element 209d placed across the joint to provide synthesisable control of the extension (q_p) along the common axis. The virtual prismatic extension control mechanism 109b-1 is an exemplary implementation of a virtual dynamic extension mechanism. As shown, the virtual inerter element 209d may be affixed at one end to the base of the instrument 601 (e.g. at the attachment point between the instrument 601 and the distal segment 202-3 of the articulated arm 105b), and at a distal end to the virtual prismatic joint 209c. Sliding motion of the virtual prismatic joint 209c may be restricted relative to the remote centre of motion (RCM) 607 using a further pair of spring-damper control mechanisms 109a- 3,109a-4 attached to the virtual prismatic joint 209c along respective Cartesian axes. [0081] The robot 105b is also configured with virtual spring-damper control mechanisms 109a-1,109a-2 attached to the end-effector 201 along respective Cartesian axes and associated with a reference location (r), similar to the spring-damper embodiment discussed above. In this way, the corresponding synthesised controller 103 allows the end-effector 201 to follow a surgeon’s input commands, for example to navigate the instrument 601 to pass through the trocar 605. The damper elements 209b, which provide stability and remove oscillations, are advantageously co-located with the spring elements 209a for efficient algorithmic parameter tuning/selection. The structure of the overall virtual instrument control mechanism 109b-2 may be constructed by progressively assembling individual components with individual functions. [0082] Referring to the exemplary virtual prismatic extension control mechanism 109b-1, an ideal virtual linear inerter element 209d may be defined as producing a force proportional to an acceleration:

^ is the acceleration between the two ends of the inerter 209c, and m is a synthesizable inertance control parameter 123-m (e.g. based in units of kg). A geared-up, low-friction flywheel is a real world mechanism that approximates an inerter: it resists acceleration at its input (or between two different input shafts), but has minimal friction, and minimal mass. Conventionally, inerters are rarely used in passivity-based control of robots, instead opting for a Proportional/Derivative approach (Proportion/Derivative from position, i.e. Proportional/Integral from velocity). This may be due to difficulty in sensing acceleration, or difficulty in actuation as acceleration has more high frequency content than position or velocity signals, so a wider bandwidth is required. An inerter 209c generalises the concept of mass. For example, the dynamics of a point-mass m can be recreated by three ideal inerters 209c, with inertance m, along Cartesian axes with one of their terminals grounded. A constant bias force mg may be included to emulate gravity. [0083] Inerters 209c are useful when paired with additional dynamic extension elements, such as revolute/prismatic joints 209c, that form part of the virtual dynamic extension control mechanism 109b. A virtual joint followed by a massless virtual link would experience infinite acceleration. One possibility may be to grant the virtual inerter link some virtual mass. However, if the acceleration of the virtual mass depends not just upon the acceleration of the virtual joint, but also the robot’s joint accelerations, then these accelerations must be measured in order to implement the inertial forces that the virtual mass would present to the system 105b. Instead, by placing a virtual inerter 209c directly across the virtual joint 209c as shown in Figure 7, the controller 103 can limit acceleration of the virtual joint 209c (no longer infinite), under the experience of inertia, and the system 101 does not have to measure joint acceleration/ implementing inertial forces. [0084] Accordingly, the virtual inerter element 209d may be defined by

where F_p defines the acceleration-force characteristic of the virtual prismatic joint 209c. ^^_^ may depend only on the force across the virtual joint 209c as the acceleration-force characteristic is not affected by any other joints of the robot 105b. The result is an extended system (robot + dynamic extension) with extended inertia matrix

which is block diagonal. Thus, the robot’s inertia is unaffected by the inertia of the virtual dynamic extension control mechanism. The apparent inertia of the physical robot is not modified by the control action, which would could be achieved through joint- acceleration measurements and higher-bandwidth control. [0085] The virtual prismatic extension control mechanism 109b-1 can be modified to allow a relaxation of the RCM constraint discussed above. For example, as schematically illustrated in Figures 8A and 8B, this can be achieved with a 3D generalization of the prismatic extension, by rotating the virtual spring-damper control mechanisms 109a-3,109a-4 on the sliding joint 209c to provide extension control along a defined axis of the surgical instrument 601, such as an axis of the surgical instrument that is normal to the instrument 601 (and the patient’s body 603), and replacing in-plane springs with a pair of piecewise-linear springs. These piecewise linear springs may have a low/zero stiffness region, e.g. defining a deadzone 801, with the effect of relaxing the point constraint of the RCM 607 to a rectangular constraint (a_x,a_y) in the specified plane of the deadzone 801. The deadzone 801 may be defined by any other suitable geometry and associated coordinates (e.g. a disk constraint rather than a rectangle). Synthesis of the piecewise springs may be implemented by modifying the linear-matrix-inequality problem formulation to include further variables (L,Q) corresponding to multiple regions. Each pose

may belong to a respective region, and each region may feature differing weighting W₁, W₂. Additional equality constraints may be used between (L,Q) from different regions to keep constant parameters which do not vary between particular pairs of regions. Alternatively, the RCM constraint can be relaxed to allow the RCM 607 to move across/along a surface or within a volume (instead of a point) in response to forces acting on the virtual spring-damper control mechanisms 109a-3,109a-4 attached at the RCM 607. Virtual Instrument [0086] In the prismatic extension mechanism of the above embodiment, the placement of the spring-dampers 109a-3,109a-4 attached to the sliding joint 209c may cause a loss in performance as the end-effector 201 approaches the port of the trocar 605. An effective ‘lever arm’ between the end effector 201 and RCM 607 decreases in length, and as such the rotational stiffness imparted by the virtual dynamic extension mechanism 109b to the surgical instrument 601 decreases. Conversely, as the surgical instrument 601 moves far away from the trocar 605 (for example, when retracted from the patient), the lever arm and apparent rotational stiffness may increase. These changes may result in a demanding torque signal for the actuator motors 203, with higher bandwidth requirements. [0087] Figure 9 schematically illustrates a robotic manipulator system 105c with another exemplary implementation of a virtual dynamic extension mechanism according to a further embodiment. As shown, the robotic manipulator system 105c of the present embodiment includes a virtual instrument control mechanism 109b-2, which is another exemplary implementation of a dynamic extension, consisting of a revolute joint fixed in the world at the RCM 607, followed by a prismatic joint 209c, with the longitudinal axis passing through the RCM 607. The virtual instrument 109b-2 may be attached to the surgical instrument 601 by three virtual spring-damper control mechanisms 109a- 6,109a-7,109a-8, and configured so that the longitudinal axis of the virtual instrument 109b-2 always passes through the RCM 607. The robot 105c also includes virtual spring-damper control mechanisms 109-1,109-2 attached to the end-effector 201 along respective Cartesian axes and associated with a reference location (r), to constrain the end-effector 201 to the surgeon reference (r) similar to the embodiments discussed above. [0088] The virtual link is endowed with mass

and inertia matrix I_i, so that the dynamic extensions inertia matrix M_c(q_c) is never singular. The virtual instrument 109b- 2 may be constrained to lie exactly on top of the surgical instrument 601 in a first position, by configuring the springs 209a with a resting length of zero, as shown in the centre of Figure 9. With the non-rigid coupling provided by the spring-dampers 109a of the virtual instrument control mechanism 109b-2, the extended-system inertia has a block diagonal structure

where M_c(q_c) is the inertia matrix of the virtual instrument control mechanism 109b-2. In the present exemplary implementation, virtual mass/inertia is used rather than inerter 209c, so that the behaviour of the virtual instrument control mechanism 109b-2 is more similar to a real instrument acting as a constrained rigid body. [0089] A further view of the virtual instrument control mechanism 109b-2 in a second, extended, position, is illustrated on the right hand side of Figure 9. As shown, the spring-dampers 109a of the virtual instrument control mechanism 109b-2 provide a more complex operation space than the standard Cartesian operation-space of the simpler spring-damper embodiments described above. For example, considering the topmost virtual spring-damper control mechanism 109a-8, the corresponding operation space coordinate component may be represented by the distance between the base of the surgical instrument 601 and the base of the virtual instrument 109b-2 in the x direction of the virtual instrument’s coordinate frame. Advantageously, the corresponding control parameters of the virtual instrument control mechanism 109b-2 may be synthesised based on the parameter tuning technique described above with reference to Figure 5, as the number of spring-dampers is equal to the degrees of freedom of the robot 105c. [0090] In this way, the virtual instrument 109b-2 addresses the potential issues identified above, by placing the spring-dampers 109a of the sliding mechanism 209c in the reference frame of the virtual instrument 109b-2, such that rotational stiffness of the surgical instrument 601 does not vary with the robot’s pose. As an alternative implementation, the reference point (r) may be defined as connected or relative to the virtual instrument 109b-2, rather than to the surgical instrument 601 or the robot 105 itself. Additionally, similar to the prismatic embodiment, the virtual instrument control mechanism 109b-2 can be modified to allow a relaxation of the port constraint, and to provide a less aggressive use of the motor torques at the joints, reducing bandwidth requirements. Virtual Dynamic Extension Controller [0091] Figure 10 is a block diagram illustrating the overall structure of an exemplary virtual mechanism controller 103b according to the present embodiment. The controller 103b of the present embodiment is based on the controller 103a described above with reference to Figure 5, adapted to implement conditions based on linear matrix inequalities that lead to control parameters 123 of the virtual spring-damper control mechanisms 109a and the virtual dynamic extension mechanisms 109b that preserve the passivity of the robot manipulator 105c. As shown, the virtual mechanism controller 103b is configured to determine the corresponding joint-torques to implement the effect of the virtual dynamic extension mechanisms 109b, which are calculated using the operation-space Jacobian transformer 125. F_d are exogenous input forces/positions and z_d are exogenous output forces/positions of the robot manipulator 105c. [0092] The dynamic extension block 109b may be implemented as an Euler-Lagrange system modelling virtual joints 209c and inerter links 209d, for example of the prismatic extension control mechanism 109b-1 and the virtual instrument control mechanism 109b-2, using the generalised coordinate q_c. The extended state

is the concatenation of q_c onto the robot state with, as represented by the multiplex (Mux) block 1003. The input u may be split between the robot’s actuators 203 and actuators of the virtual dynamic extension mechanism 109b by de-multiplexing, as represented by the DeMux block 1005. The actuator commands u are sent to the robot 105c, and the virtual dynamic extension mechanism 109b state is updated in the virtual mechanism controller 103b, according to the dynamic equation in generalised coordinates q_c

inertia matrix and C_c is the inertia matrix of the virtual mechanism controller 103b. The block-diagonal structure of the extended inertia matrix, with off diagonal blocks being zero, is described above with reference to the respective exemplary implementations of the virtual dynamic extension mechanism. Virtual dynamic extension mechanisms may be configured without the separation of inertia, if additional inputs for the extra inertial forces are provided to both the robot and dynamic extension. [0093] Also shown are components to synthesise control parameters 123 of the springs 209a and dampers 209b of the virtual mechanism, similar to the embodiments described above with reference to Figure 5. In the present exemplary implementation, a transform (H_c) function block 1007 may be defined as:

where is a defined forward kinematics function which returns the extension z_c of the virtual mechanism’s springs 209a, and its partial derivative gives the Jacobian

[0094] J_c represents the interconnecting Jacobian transformer 125 shown in Figure 1, with extra columns to relate motion/forces from/to the generalised coordinates of the dynamic extension. The controller force F_c is the combined force of all springs 209a and dampers 209b in the operation space of the controller 103b. If the springs and dampers are linear, then the general formulation

can be replaced with

where k is the vector of spring-stiffness control parameters 123-k, and c is the vector of damping coefficient control parameters 123-b. K represents a linear state-feedback matrix in operation space z_c. The operation space coordinates may be designed such that z_c = 0 is the desired equilibrium, by including the appropriate constant offset in the transform H_c. [0095] Having established the robot model of the controller 103b in operation space, the constructed controller 103b may proceed to determine selection of the corresponding controller parameters/gains as discussed above with reference to steps S3-5 to S3-7 of Figure 3. Experimental Simulation Examples [0096] As a first example, we illustrate our energy shaping and damping injection design experimentally on a two link manipulator 105a as shown in Figures 2 and 4B, with links of length 1m, each weighing 3Kg, and virtual spring-damper control mechanisms 109a-1,109a-2 aligned to respective Cartesian axes, pulling the end- effector 201 position z_c towards a desired reference r = [1.0; 1.0]. The corresponding controller model is stated by equations (2) and (3) above. [0097] For simplicity, we assume that the disturbing force acts on the end-effector 201 as well, that is, z_d = z_c. We choose the following weighting matrices, which add a larger weight to vertical displacement/velocity errors. In a pick and place task, this choice could be motivated by the desire of reaching a tighter control of the vertical position of the robot, to compensate for uncertainties on the load.

[0098] We also consider three poses, each requiring the end-effector to be in the upper right quadrant.

corresponds to the reference point r.

[0099] We construct the modified LMI problem for two different gains: Ȗ = 10.0, and Ȗ = 0.1. As discussed above, the choice of Ȗ determines the level of performance, with smaller Ȗ corresponding to tighter position control (higher stiffness of the robot). Solving the LMIs with CVX leads to the following controller gains:

[00100] The stiffness coefficients of the virtual vertical spring-damper control mechanisms 109a-2 is always larger than the stiffness coefficients of the virtual horizontal spring-damper control mechanisms 109a-1, in agreement with the weighting functions W₁, and both stiffness coefficients become larger for Ȗ = 0.1, as expected. The virtual damping coefficients are larger in the x direction. This can be explained by the greater inertia of the manipulator arm 105a in that direction, considering the poses specified in the synthesis. Moving the end-effector 201 in the y direction requires accelerating predominantly the second link, whereas moving the end-effector 201 in the x direction requires accelerating both links, with a higher combined inertia. [00101] The control performance is illustrated in Figure 11. According to the simulated results, the step disturbance force (F_d) acts first in the x direction, then in the y direction. Larger displacement errors _y correspond to the case Ȗ = 10, as

expected. In both cases, į_y is smaller than į

, in agreement with the weights W₁ and W₂. [00102] As another example, we show that the controller 103 is scalable to control a robotic manipulator 105 with a greater number of degrees of freedom. Figure 12 shows an example of a redundant arm 105d, the KUKA LBR iiwa 14 R820 seven-axis (rotational joints) robot, as included in the MATLAB Robotics System Toolbox. We define the kinematics as a hybrid, seven dimensional operation-space comprised of three elements. The first element is the end-effector 201d Cartesian position, z_ee(t). The second element is the elbow height, represented by the z coordinates of the elbow (fourth) link 202d-4, z_el(t). The third element is the vector of joint angles, z_q(t), related to the final three joints 203d-5, 203d-6 and 203d-7 of the arm 105d. These angles can be used to set the orientation of the end-effector 201d. Stacking all three vectors together forms

[00103] For the disturbing force F_d we consider z_d = [z_ee z_q], and we choose weighting matrices

which removes the displacement of the elbow 202d-4 from our performance criterion. The controller 103 is synthesised around pose q^כ, shown in Figure 12. q^כ is not near any extrema of the workspace or singularities. The reference position r satisfies ^ ൌ ^_^ ^כ ൌ that is

[00104] Using the modified LMI problem set out above, we derive the controller gains below. In this simulated example, a virtual spring and damper controller is located at the end effector 201d, elbow 202d-4, and final three joints 203d-5, 203d-6 and 203d-7 of the robot 105d, but the dynamic model 109 of the present embodiment advantageously allows for free placement of the virtual spring-damper control mechanisms 109 at any location about the robot manipulator 105d. The corresponding control parameter coefficients are obtained near q^כ for Ȗ = 10 and Ȗ = 1. Indeed, larger gains corresponds to Ȗ = 1, as expected.

[^{00105] The arm is subject to force That is, the}

force acts only on the end-effector in direction [111], with magnitude 10N. The control performance according to these simulated results is illustrated in Figure 13. As shown, the norm of the position error at the end-effector 201d,

and the norm of the error in joint angles į ^T There is a transient error

in orientation both at the onset and removal of the disturbance, and a step error in position. Decreasing Ȗ increases the synthesised gains and reduces the magnitude of the errors. [00106] In a third example, we use a Unified Robot Description Format (URDF) model of the Franka-Emika robot arm 105e, modified to include a surgical instrument 601 as shown in Figure 14A. The simulated robot 105e also includes a virtual prismatic extension control mechanism 109b-1 (as discussed above with reference to Figure 7), with inertance m_c = 0.1 kg, and a virtual instrument control mechanism 109b-2 (as discussed above with reference to Figure 9) with mass m_i = 0.1 kg and inertia I_i = . These values are chosen so as to be small enough to not add

lots of extra inertia, but not so small as to greatly increase the number of required simulation timesteps. The virtual instrument control mechanism 109b-2 is not shown in Figure 14 for clarity. [00107] For the relevant exogenous inputs/outputs (shown as arrows at respective ends of the instrument 601), we will first consider the input given by a disturbance force acting on the end-effector 201e, taking as output its position/velocity. We will also consider an input force applied at the base of the instrument 601 (i.e. the end attached to the end effector 201e) and the associated position/velocity output. The inputs and outputs are combined into 6 × 1 vectors of forces and displacements, and velocities, F_d, z_d and ^^_ௗ. The inputs/outputs capture the requirements that there should be high impedance from force to motion at the end effector 201e, and small motion at the trocar 605. We will take Ȗ = 1, W₂ = 0, and vary W₁. [00108] As discussed above, the controller parameter synthesis technique is implemented based on the number of virtual spring-damper control mechanisms 109a being the same as the degrees of freedom (DOF) of the closed-loop system (i.e. DOF of robot + DOF of virtual mechanism = number of spring-dampers). The Franka-Emika robot 105e has seven degrees of freedom. The virtual prismatic extension control mechanism 109b-1 adds one further joint/DOF and four virtual spring-damper control mechanisms 109a. The virtual instrument control mechanism 109b-2 adds three further joints/DOF and five further virtual spring-damper control mechanisms 109a. Therefore, the combination of virtual control mechanisms 109 in this simulated example requires two additional virtual spring-damper control mechanisms 109a, for the above-described parameter synthesis technique to be applied. We choose to use joint-space spring- dampers on the second and fourth joints (J2 and J4, relative to the base), with a resting position at ʋ/6 and -4ʋ/6 respectively. These particular choices of J2/J4 angles are to keep the arm upright and with J4 folded, but away from singularities. [00109] The set of joint poses q_i about which the robot's dynamics will be linearised and performance optimized, are chosen as shown in Figure 14B: nine poses passing through an RCM at (0.4, 0, 0.35) to place the end-effector 201e on an evenly spaced 3 × ^

J2 and J4 equal to ʌ/6 and 4ʌ/6 respectively. The poses should be chosen to avoid singularities. If the controller Jacobian is near singular, then the gains required to achieve a particular performance at that pose will be large. The operation space variables (i.e. representing spring-damper placement) for each mechanism in the simulated example robot 105e shown in Figure 14A are summarised in Tables 1 and 2. [00110] Table 1 shows a summary of controller operation variables for the placement of virtual spring-damper control mechanisms 109a of the virtual prismatic extension control mechanism 109b-1.

Table 1: Prismatic extension spring-damper placement [00111] Table 2 shows a summary of controller operation variables for the placement of virtual spring-damper control mechanisms 109a of the virtual instrument control mechanism 109b-2.

Table 2: Virtual instrument spring-damper placement [00112] Table 3 shows the results of the synthesis of respective stiffness (k) and damping (b) control parameters 123 for the virtual prismatic extension control mechanism 109b-1.

Table 3: Synthesized stiffness (k) and damping (b) parameters of prismatic extension [00113] Table 4 shows the results of the synthesis of respective stiffness (k) and damping (b) control parameters 123 for the virtual instrument control mechanism 109b- 2, across two choices of W₁.

2 1780 349 9930 779 3 1150 172 7230 432 4 0.152 0.782 0.149 0.809 5 0.156 1.25 0.157 1.41 6 428 45.9 3000 119 7 1470 306 10500 817 8 0.0515 0.0913 0.0513 0.0871 9 884 89.8 5060 197 10 2860 727 15200 1570 Table 4: Synthesized stiffness (k) and damping (b) parameters of virtual instrument [00114] Note that the stiffness/damping parameters for J2, J4 and the instrument- aligned spring-damper for the virtual instrument mechanism are small. The synthesis ensures that they are greater than zero such that the linearisation is stable, but they are not necessary for minimising the gain between the chosen F_d and z_d. Increasing W₁ has the desired effect of increasing the overall gains. [00115] We performed simulations on each set of gains, using the URDF model of the robot 105e following the desired end-effector trajectory:

as shown in Figure 15A. [00116] Figure 15B shows the results of the simulation, where the absolute error between the end-effector 201e tip is plotted alongside the minimum distance between the axis of the instrument 601 shaft and the remote centre of motion. Joint torques are shown without gravity compensation, which would otherwise dominate the plot. The virtual prismatic extension control mechanism 109b-1 and virtual instrument control mechanism 109b-2 perform similarly, when using gains synthesized with the same weighting and exogenous inputs/outputs. The increased gains as a result of setting W₁ to 100 reduce the error further, at the cost of a higher bandwidth controller. [00117] We also present results from a simulation using the rectangular constraint of Figure 8. The X and Y Cartesian springs of the prismatic extension (i.e. on the 6th and 7th coordinates of z_c) are replaced with piecewise springs, having zero stiffness regions of width/depth a_x = a_y = 30mm, with the outer stiffness taken from Table 1. The corresponding dampers have their damping coefficients reduced to 5% of the values in Table 1. All other spring-dampers are unchanged. Figure 15C shows an improvement in the end-effector performance, as compared with the first column of Figure 15B, due to the relaxation of the RCM constraint. Example Computer System Implementation [00118] Figure 16 illustrates an example computer system 1600 in which the present invention, or portions thereof, can be implemented as computer-readable code to program processing components of the computer system 1600. Various embodiments of the invention are described in terms of this example computer system 1600. For example, the robot control system architecture 101 of Figure 1 can be implemented in system 1600. The method illustrated by the flowchart of Figure 3 can also be implemented in system 1600. After reading this description, it will become apparent to a person skilled in the relevant art how to implement the invention using other computer systems and/or computer architectures. [00119] Computer system 1600 includes one or more processors, such as processor 1604. Processor 1604 can be a special purpose or a general-purpose processor. Processor 1604 is connected to a communication infrastructure 1606 (for example, a bus, or network). Computer system 1600 also includes a user input interface 1603 connected to one or more input device(s) 1605 and a display interface 1607 connected to one or more display(s) 1609, which may be integrated input and display components. Input devices 1605 may include, for example, a joystick, a pointing device such as a mouse or touchpad, a keyboard, a touchscreen such as a resistive or capacitive touchscreen, etc. According to an embodiment, computer display 1630, in conjunction with display interface 1602, can be used to display a user interface to an operator of the system. [00120] Computer system 1600 also includes a main memory 1608, preferably random access memory (RAM), and may also include a secondary memory 1610. Secondary memory 1610 may include, for example, a hard disk drive 1612, a removable storage drive 1614, flash memory, a memory stick, and/or any similar non-volatile storage mechanism. Removable storage drive 1614 may comprise a floppy disk drive, a magnetic tape drive, an optical disk drive, a flash memory, or the like. The removable storage drive 1614 reads from and/or writes to a removable storage unit 1618 in a well- known manner. Removable storage unit 1618 may comprise a floppy disk, magnetic tape, optical disk, etc. which is read by and written to by removable storage drive 1614. Removable storage unit 1618 includes a non-transitory computer usable storage medium having stored therein computer software and/or data. [00121] In alternative implementations, secondary memory 1610 may include other similar means for allowing computer programs or other instructions to be loaded into computer system 1600. Such means may include, for example, a removable storage unit 1622 and an interface 1620. Examples of such means may include a program cartridge and cartridge interface (such as that found in video game devices), a removable memory chip (such as an EPROM, or PROM) and associated socket, and other removable storage units 1622 and interfaces 1620 which allow software and data to be transferred from the removable storage unit 1622 to computer system 1600. [00122] Computer system 1600 may also include a communications interface 1624 implemented for example at the operating system level to allow data to be transferred between computer system 1600 and external devices, for example as signals 1628 over a communication channel 1626. Communications interface 1624 may include a modem, a network interface (such as an Ethernet card), a communications port, a PCMCIA slot and card, or the like. [00123] Various aspects of the present invention can be implemented by software and/or firmware (also called computer programs, instructions or computer control logic) to program programmable hardware, or hardware including special-purpose hardwired circuits such as application-specific integrated circuits (ASICs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), etc. of the computer system 1600, or a combination thereof. Computer programs for use in implementing the techniques introduced here may be stored on a machine-readable storage medium and may be executed by one or more general-purpose or special-purpose programmable microprocessors. The terms “computer program medium”, “non-transitory computer readable medium” and “computer usable medium” introduced herein can generally refer to media such as removable storage unit 1618, removable storage unit 1622, and a hard disk installed in hard disk drive 1612. Computer program medium, computer readable storage medium, and computer usable medium can also refer to memories, such as main memory 1608 and secondary memory 1610, which can be memory semiconductors (e.g. DRAMs, etc.). These computer program products are means for providing software to computer system 1600. [00124] Computer programs are stored in main memory 1608 and/or secondary memory 1610. Computer programs may also be received via communications interface 1624. Such computer programs, when executed, enable computer system 1600 to implement the present invention as described herein. In particular, the computer programs, when executed, enable processor 1604 to implement the processes of embodiments of the present invention as described above. Accordingly, such computer programs represent controllers of the computer system 1600. Where the invention is implemented using software, the software may be stored in a computer program product and loaded into computer system 1600 using removable storage drive 1614, interface 1620, hard drive 1612, or communications interface 1624. [00125] Embodiments of the invention employ any computer useable or readable medium, known now or in the future. Examples of computer useable mediums include, but are not limited to, primary storage devices (e.g., any type of random access memory), secondary storage devices (e.g., hard drives, floppy disks, CD ROMS, ZIP disks, tapes, magnetic storage devices, optical storage devices, MEMS, nano- technological storage device, etc.), and communication mediums (e.g., wired and wireless communications networks, local area networks, wide area networks, intranets, etc.). Alternative Embodiments [00126] It will be understood that embodiments of the present invention are described herein by way of example only, and that various changes and modifications may be made without departing from the scope of the invention. [00127] For example, in the embodiments described above, the LMI-based synthesis implements a diagonal structure of the coefficient solution matrices, which may limit the degrees of freedom of the constructed controller. It is possible to generalise the sparsity patterns to allow for more degrees of freedom while preserving closed-loop passivity. For example, the structural assumption of diagonal matrices allows for a controller that preserves passivity, as long as the constraint Q₁₂ ط 0 is applied in the modified LMI problem. This assumption could be generalised to leave more freedom on the structure of Q and L in the modified LMI problem. [00128] The weighting matrices set out in (5) can be extended to weighting transfer matrices, which would apply a nonuniform weighting to the frequency content of ^_ௗ and ^^_ௗ. The linearization defined in model (7) would need to be further modified to include additional dynamics corresponding to a minimal realization of

and W₂(s). The model would also need to be modified to enforce zero sub-blocks constraints in the matrices Q and L of the modified LMI problem, to obtain a state feedback of the right dimension. [00129] In the embodiments and exemplary implementations described above, the controller design implements N virtual stiffness and N virtual damping parameters, where N are the degrees of freedom of the robot manipulator. As those skilled in the art will appreciate, the precise numbers of virtual control mechanisms described in the exemplary embodiments above are merely illustrative and the number and complexity of virtual control mechanisms are configurable depending on the specific implementation context, for example to modify the resulting robot control flexibility and/or performance. The virtual control mechanism approach of the present embodiments allows an arbitrarily greater number of springs and dampers, lending greater flexibility and better performance. For example, the dynamic model may be adapted such that the number of control parameters is larger than the degrees of freedom of the robot. [00130] Additionally, synthesis of control parameters for a number of virtual spring- damper control mechanisms greater than n may be further modified for improved computational efficiency by updating n parameters at a time (using the linear matrix inequality synthesis technique described above) while keeping fixed the other parameters. At each iteration, a different set of n spring-damper control parameters may be fixed, until the whole set of parameters converges to a stable solution., Another alternative approach to virtual control mechanism parameter tuning is by implementing a gradient-based approach, for example using an automatic differentiation tool, to minimize a chosen cost function over a trajectory or set of trajectories. Such gradient based approaches are typically less restrictive than the linear matrix inequality approach, thereby advantageously allowing more flexible optimization over any number of parameters, types of controller parameter (e.g. inertances, masses, geometries), and non-linear components. As those skilled in the art will appreciate, passivity from the virtual control mechanisms is ensured so long as the determined virtual control mechanism structure is maintained and a corresponding appropriate cost function is defined. [00131] As yet another alternative, the controller and constructor modules and respective sub-modules may be provided as one or more distributed computing modules or processing services on a remote server that is in communication with the robot control system via a data network. The constructor module functionality may be provided as one or more application programming interface (API) accessible by an application program executing on the robot control system, or as a plug-in module, extension, embedded code, etc., configured to communicate with an application program. [00132] Reference in this specification to “one embodiment” are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. In particular, it will be appreciated that aspects of the above described embodiments can be combined to form further embodiments. Similarly, various features are described which may be exhibited by some embodiments and not by others. Yet further alternative embodiments may be envisaged, which nevertheless fall within the scope of the following claims. [00133] In the foregoing specification, the invention has been described with reference to specific exemplary embodiments thereof. It will, however, be evident that various modifications and changes may be made thereto without departing from the broader scope of the invention as set forth in the appended claims. The specification and drawings are, accordingly, to be regarded in an illustrative rather than a restrictive sense.

Claims

CLAIMS 1. A method to synthesise a controller of a robotic manipulator system using a model defining a plurality of control parameters associated with one or more virtual control mechanisms attached to respective locations of the robotic manipulator system, wherein output control actions of the model are defined based on said plurality of control parameters of the one or more virtual control mechanisms, the method comprising: determining linear matrix inequality, LMI, constraints for synthesis of control parameters that achieve a defined performance, wherein the LMI constraints define a gain matrix, K, comprising a plurality of diagonal matrices that correspond to the plurality of control parameters; deriving a solution to the LMI constraints, wherein the solution identifies control parameter values in the plurality of diagonal matrices; and determining output control actions for one or more actuators of the robotic manipulator system based on the identified control parameter values. 2. The method of claim 1, wherein the virtual control mechanisms comprise a plurality of virtual spring-damper control mechanisms, and wherein the gain matrix has the form K [K K ], where defines virtual stiffness control parameters and K

₂

defines virtual damping control parameters of the virtual spring-damper control mechanisms. 3. The method of claim 1 or 2, further comprising a feedback control loop to optimize performances on a performance pair defined by input exogenous forces,

and output exogenous displacements,

4. The method of claim 3, wherein the LMI constraints are determined for synthesis of control parameters that achieve a performance metric based on an or

control criterion. 5. The method of any preceding claim, wherein deriving a solution to the LMI constraints comprises linearization of the model in operation space. 6. The method of claim 5, wherein the robot model is obtained based on a control law for the one or more actuators having the form:

where represents an operational force vector resulting from the optimal control parameter values in combination with the dynamics of a generalised plant,

represents the placement of the virtual control mechanisms, r represents a desired reference position, and represents a Jacobian transform.

7. The method of claim 6, wherein the linearization is defined by the linearized state-space system:

where:

8. The method of claim 7, wherein the LMI constraints further comprise matrices:

where i denotes the matrices associated with the linearization , and Ȗ is an

input parameter ^ defining a level of performance to be achieved, and wherein convex optimisation is performed to find

such that: ǡ ǡ ǡ ǡ ll di l i

9. The method of any preceding claim, wherein the number of virtual control parameters is less than or equal to the degrees of freedom of the robotic manipulator system. 10. The method of any preceding claim, wherein the virtual control mechanisms further comprise a virtual dynamic extension mechanism having a prismatic joint component to provide extension control along a defined axis of the robotic manipulator system. 11. The method of claim 10, wherein the defined axis is a common axis of an instrument attached to the robotic manipulator system. 12. The method of claim 11, wherein a first pair of virtual spring-damper control mechanisms are attached to an end-effector of the instrument, and a second pair of virtual spring-damper control mechanisms are attached to the virtual dynamic extension mechanism. 13. The method of claim 12, wherein the second pair of virtual spring-damper control mechanisms are rotated about the prismatic joint component of the virtual dynamic extension mechanism to align the extension control with a defined direction. 14. The method of any one of claims 10 to 13, wherein the defined axis is an axis of a virtual instrument attached to a physical instrument by a plurality of virtual spring- damper control mechanisms. 15. The method of claim 14, wherein a further pair of virtual spring-damper control mechanisms are attached to an end-effector of the physical instrument. 16. The method of claim 2 or any claim dependent thereon, wherein each virtual spring-damper control mechanism comprises a co-located spring and damper component. 17. A controller for a surgical robot having a surgical instrument supported by an articulated robot arm, the controller being configured to: synthesise control of one or more virtual control mechanisms attached to respective locations of the surgical robot, to effect movement of the surgical instrument through a port location; and determine control actions of the articulated robot arm based on the synthesised control of the one or more virtual control mechanisms. 18. The controller of claim 17, wherein the virtual control mechanisms comprise a virtual dynamic extension mechanism having a prismatic joint component to provide extension control along a defined axis of the surgical instrument. 19. A system comprising one or more processors configured to perform the method of any one of claims 1 to 16. 20. A storage medium comprising machine readable instructions stored thereon for causing a computer system to perform a method in accordance with any one of claims 1 to 16.