Cascade control system and method and minimally invasive surgery robot

Abstract

The embodiment of the invention relates to the technical field of minimally invasive surgery robots, and discloses a cascade control system and method and a minimally invasive surgery robot. The system comprises a controller and a driver, wherein the controller comprises a speed planning module and a control module; the control module comprises a position ring and a speed ring; the driver comprises a current loop; the speed planning module is used for determining a planning position and a planning speed; the position ring is used for outputting a reference speed to the speed ring according to the planned position; the speed ring is used for determining a reference current according to the reference speed and the planned speed and transmitting the reference current to the driver; and the current loop is used for outputting current to a controlled motor according to the reference current. And at least some technical problems existing in single-layer impedance control and single-stage PI control in related technologies can be solved.

Description

Technical Field

[0001] This application relates to the field of minimally invasive surgical robot technology, and in particular to a cascade control system, method and minimally invasive surgical robot. Background Technology

[0002] In robot joint control, direct-drive joints with small or no reduction ratios are quite common. In this case, if the position control of the joint is achieved solely through the current loop of the servo driver and the host computer, problems such as slow convergence speed and insufficient holding force after reaching the target position often occur.

[0003] Specifically, in related technologies, when relying solely on the current loop of the servo driver, there are two methods to achieve the positioning and holding functions of the target joint. The first is single-layer impedance control, which treats the controlled object as a spring-damped mass system. By adjusting the force feedforward parameters and the system's own spring damping coefficient, the system's response characteristics can be flexibly adjusted to achieve the desired control effect. The second is single-stage PI control. This method works by setting a PI controller between the target value and the current value to achieve target position tracking.

[0004] However, the inventors discovered at least the following technical problems in the related technologies: In specific application scenarios, single-layer impedance control has significant limitations due to factors such as bandwidth: First, the convergence speed is difficult to control, failing to achieve the desired effect; second, it is prone to insufficient correction force when subjected to interference. Furthermore, single-stage PI control cannot simultaneously resolve the contradiction between static error and dynamic response, and it has a fault threshold during high-speed commutation. Summary of the Invention

[0005] One objective of this application is to provide a cascade control system, method, and minimally invasive surgical robot, at least to solve the aforementioned technical problems existing in single-layer impedance control and single-stage PI control in related technologies.

[0006] To achieve the above objectives, some embodiments of this application provide the following aspects:

[0007] In a first aspect, some embodiments of this application also provide a cascade control system, the system including a controller and a driver, the controller including a speed planning module and a control module; the control module including a position loop and a speed loop; the lower-level driver including a current loop; the speed planning module for determining a planned position and a planned speed; the position loop for outputting a reference speed to the speed loop according to the planned position; the speed loop for determining a reference current according to the reference speed and the planned speed, and transmitting the reference current to the driver; the current loop for outputting current to the controlled motor according to the reference current.

[0008] Secondly, some embodiments of this application also provide a cascade control method, which is applied to the system described above. The method includes: speed planning performed by a host controller: generating a planned position and a planned speed based on a target position and a current position; position control performed by a position loop: performing PID calculations based on the deviation between the planned position and the current position, and outputting a reference speed to the speed loop; speed control performed by the speed loop: fusing the reference speed and the planned speed to generate a target reference speed, and generating a reference current based on the deviation between the target reference speed and the current speed; and sending the reference current to the current loop of a lower-level driver, and driving the motor through the current loop to achieve joint motion control.

[0009] Thirdly, some embodiments of this application also provide a minimally invasive surgical robot, the minimally invasive surgical robot comprising: one or more processors; and a memory storing computer program instructions, which, when executed, cause the processor to perform the steps of the method described above.

[0010] Compared with related technologies, the solution provided in this application adopts a hierarchical closed-loop design. The upper-level controller (including a speed planning module, a position loop, and a speed loop) and the lower-level driver (including a current loop) form a hierarchical architecture of "upper-level planning - middle-level adjustment - lower-level execution," creating a complete position-speed-current closed loop. Specifically, the controller's speed planning module outputs the planned position and planned speed; the position loop outputs a reference speed based on the planned position; the speed loop outputs a reference current based on the planned speed and the reference speed; and the current loop adjusts the output current to the controlled motor based on the reference current, completing the entire control chain from planning to execution.

[0011] On the one hand, because the hierarchical architecture in this embodiment separates planning and execution, the controller is dedicated to fine-grained planning based on the target and the current state. Through hierarchical adjustment between the speed planning module, the position loop, and the speed loop, the coupling interference between planning and execution caused by bandwidth limitations in single-layer impedance control can be avoided, thus overcoming the bottleneck of difficult convergence speed control. The current loop of the driver, as the bottom-level execution unit, can achieve real-time current adjustment with its high-frequency response characteristics, thereby solving the static error and dynamic response imbalance problem caused by incomplete closed-loop control (lack of full-link feedback of position-speed-current) in single-level PI control. On the other hand, the complete position-speed-current closed loop in this embodiment can form a multi-level feedback correction mechanism: the position loop ensures that the motion direction does not deviate from the planned trajectory, the speed loop dynamically corrects the speed deviation, and the current loop compensates for the disturbance at the execution end in real time. The three-layer collaboration can greatly enhance the anti-interference capability of the system, thus avoiding the defect of insufficient correction force in single-layer control.

[0012] It is worth mentioning that in this embodiment, even under conditions of direct motor drive and low load, the host controller can achieve precise position control solely through the current loop of the driver. Simultaneously, this solution ensures fast system convergence and allows the joint to converge to the target position and remain stable at a specified speed and acceleration, without insufficient correction force. Through the aforementioned hierarchical cascade control innovation, this application systematically addresses the shortcomings of existing technologies and meets the stringent requirements of the master finger rotary joint for motion accuracy, response speed, and stability. Attached Figure Description

[0013] One or more embodiments are illustrated by way of example with reference numerals in the accompanying drawings. These illustrations do not constitute a limitation on the embodiments. Elements with the same reference numerals in the drawings are denoted as similar elements. Unless otherwise stated, the figures in the drawings are not to be limited by scale.

[0014] Figure 1 An exemplary architecture diagram of a cascade control system provided for some embodiments of this application;

[0015] Figure 2 An exemplary flowchart of a cascade control method provided for some embodiments of this application;

[0016] Figure 3 An exemplary structural diagram of a minimally invasive surgical robot provided for some embodiments of this application. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0018] First Embodiment

[0019] The first embodiment of this application relates to a cascade control system. For example... Figure 1 As shown, the system may include a controller and a driver; the controller may include a speed planning module and a control module; the control module includes a position loop and a speed loop; the lower-level driver includes a current loop;

[0020] The speed planning module is used to determine the planned location and planned speed;

[0021] The position loop is used to output a reference speed to the speed loop based on the planned position;

[0022] The speed loop is used to determine a reference current based on the reference speed and the planned speed, and to transmit the reference current to the driver;

[0023] The current loop is used to output current to the controlled motor according to the reference current.

[0024] The following sections will provide a detailed explanation of each of the above modules.

[0025] Specifically, the system mainly consists of two parts: a controller (upper-level controller) and a driver (lower-level driver). The upper-level controller is used for motion planning and logic control, and includes a speed planning module and a control module; the lower-level driver is used for the underlying power output, and the two work together to form a complete control closed loop.

[0026] The velocity planning module is used to determine the planned position and planned velocity, providing basic motion trajectory reference parameters for the position loop and velocity loop, and ensuring that the joint movement has a clear initial planning target.

[0027] The position loop of the control module serves as the outer loop. Based on the planned position output by the speed planning module and combined with the current position of the motor, it performs adjustment calculations using a PID algorithm to output a reference speed.

[0028] In this control module, the speed loop serves as the inner loop. It can first superimpose the planned speed obtained by the speed planning module with the reference speed output by the position loop to form a new reference speed that better meets the actual needs. Then, combined with the current speed of the motor, it can be further calculated using a PID algorithm to determine the reference current.

[0029] For example, the reference current calculated by the host controller is transmitted to the current loop, which can adjust in real time according to the reference current and output a corresponding current to the controlled motor. Since the rotary joints of the master fingers in minimally invasive surgical robots have extremely high requirements for motion accuracy, response speed, and anti-interference capability, the hierarchical collaborative cascade control mode provided in this embodiment can ensure that the joint converges precisely to the target position, flexibly control the convergence speed, and enhance anti-interference capability, effectively avoiding deviations, thereby achieving high-precision control of the rotary joint.

[0030] Understandably, in certain application scenarios, due to factors such as bandwidth, single-layer impedance control, while enabling the joint to converge to the target position, suffers from difficulty in controlling the convergence speed, making it hard to achieve the desired effect. Furthermore, its correction force is insufficient when subjected to disturbances, easily leading to deviations. Single-stage PI control, lacking a complete position-velocity-current closed loop, not only fails to balance the contradiction between static error and dynamic response but also exhibits a fault threshold during high-speed commutation, making it prone to triggering faults.

[0031] It is not difficult to see that, compared with related technologies, the solution provided in this application adopts a hierarchical closed-loop design. The upper-level controller (including a speed planning module, a position loop, and a speed loop) and the lower-level driver (including a current loop) form a hierarchical architecture of "upper-level planning - middle-level adjustment - lower-level execution," creating a complete position-speed-current closed loop. Specifically, the controller's speed planning module outputs the planned position and planned speed; the position loop outputs a reference speed based on the planned position; the speed loop outputs a reference current based on the planned speed and the reference speed; and the current loop adjusts the output current to the controlled motor based on the reference current, completing the entire control chain from planning to execution.

[0032] On the one hand, because the hierarchical architecture in this embodiment separates planning and execution, the controller is dedicated to fine-grained planning based on the target and the current state. Through hierarchical adjustment between the speed planning module, the position loop, and the speed loop, the coupling interference between planning and execution caused by bandwidth limitations in single-layer impedance control can be avoided, thus overcoming the bottleneck of difficult convergence speed control. The current loop of the driver, as the bottom-level execution unit, can achieve real-time current adjustment with its high-frequency response characteristics, thereby solving the static error and dynamic response imbalance problem caused by incomplete closed-loop control (lack of full-link feedback of position-speed-current) in single-level PI control. On the other hand, the complete position-speed-current closed loop in this embodiment can form a multi-level feedback correction mechanism: the position loop ensures that the motion direction does not deviate from the planned trajectory, the speed loop dynamically corrects the speed deviation, and the current loop compensates for the disturbance at the execution end in real time. The three-layer collaboration can greatly enhance the anti-interference capability of the system, thus avoiding the defect of insufficient correction force in single-layer control.

[0033] It is worth mentioning that in this embodiment, even under conditions of direct motor drive and low load, the host controller can achieve precise position control solely through the current loop of the driver. Simultaneously, this solution ensures fast system convergence and allows the joint to converge to the target position and remain stable at a specified speed and acceleration, without insufficient correction force. Through the aforementioned hierarchical cascade control innovation, this application systematically addresses the shortcomings of existing technologies and meets the stringent requirements of the master finger rotary joint for motion accuracy, response speed, and stability.

[0034] Second Embodiment

[0035] The second embodiment of this application relates to a cascade control system. The second embodiment is an improvement upon the first embodiment, specifically in that it provides a concrete implementation of a speed planning module.

[0036] Optionally, the speed planning module may specifically include a first determining unit and a second determining unit:

[0037] The first determining unit is used to determine the position-velocity curve based on the current position and the target position of the controlled motor;

[0038] The second determining unit is used to determine the planned position and planned speed based on the position-velocity curve.

[0039] Specifically, the first determining unit can obtain the difference between the current position and the target position of the controlled motor, and plan a smooth position-velocity curve between the target position and the current position based on the difference, so as to ensure that the changes in speed and acceleration of the motor when it moves from the current position to the target position meet the system requirements.

[0040] For example, the position-velocity curve can be, but is not limited to, a trapezoidal curve or an S-shaped curve. In the trapezoidal curve, the velocity first accelerates uniformly to a rated value, then accelerates uniformly, and finally decelerates uniformly to zero; in the S-shaped curve, the velocity change is smoother, and the acceleration exhibits a gradual change characteristic. The specific curve type can be selected according to the stability requirements of the actual motion scenario, and this embodiment does not impose specific limitations on it.

[0041] For example, the second determining unit can use the position-velocity curve generated by the first determining unit as a reference and perform real-time sampling according to the system control cycle (e.g., once every millisecond). At each sampling moment, the position and velocity values ​​corresponding to that moment can be calculated using the functional expression (e.g., a piecewise polynomial) of the position-velocity curve, and used as the planned position and planned velocity, respectively; if discretized curve data storage is used, the pre-stored position and velocity parameters corresponding to that moment can be directly read. These extracted planned positions and planned velocities are synchronously output to the position loop and velocity loop of the control module, providing a real-time updated trajectory reference for subsequent closed-loop adjustment. For example, when the system control cycle is set to 1 millisecond, the second determining unit can use the S-shaped position-velocity curve generated by the first determining unit as a reference and perform sampling every 1 millisecond. If the current position is in the acceleration phase of the position-velocity curve, the second determining unit can calculate the position value (e.g., 5.2 mm at the 10th millisecond) and velocity value (e.g., 3 mm / s at the 10th millisecond) corresponding to this moment using a polynomial function (e.g., a cubic polynomial) for this phase, and use them as the planned position and planned velocity, respectively. If the position-velocity curve is stored in a discretized manner (e.g., the position and velocity data corresponding to each millisecond are pre-calculated and stored), the pre-stored data corresponding to the 10th millisecond can be read directly (e.g., position 5.2 mm, velocity 3 mm / s).

[0042] Optionally, in some embodiments, the first determining unit may include:

[0043] The displacement calculation subunit is used to determine the total displacement based on the difference between the current position and the target position of the controlled motor.

[0044] The curve type selection sub-unit is used to determine the type of piecewise planning curve based on the system's requirements for motion smoothness and response speed.

[0045] The parameter configuration subunit is used to configure the motion parameters corresponding to the segmented planning curve type, including the maximum allowable speed, the maximum allowable acceleration, the jerk, and the duration percentage of each motion stage.

[0046] The curve generation sub-unit is used to determine the position-velocity curve based on the total displacement, the segmented planning curve type, and the motion parameters.

[0047] For example, the displacement calculation subunit can collect the current position data of the controlled motor in real time through a motor encoder or position sensor, and at the same time receive the target position command (usually an absolute position or relative displacement command) issued by the system. Then, it performs vector operation on the target position and the current position (if multi-axis motion is involved, coordinate system transformation needs to be considered). The difference between the two is calculated by subtraction to obtain the displacement with direction information. The absolute value of the difference is taken (or the sign is retained to reflect the direction of motion) to determine the total displacement from the current position to the target position.

[0048] For example, the curve type selection subunit can first receive performance requirement parameters from the upper system, including motion smoothness level (e.g., no impact, low impact, normal impact) and response speed threshold (e.g., maximum allowable motion time), and then match them through preset decision logic. Specifically, if the requirement is high response speed and slight impact is acceptable (e.g., rapid positioning scenario), a trapezoidal curve (three-stage switching of acceleration, constant speed, and deceleration, with acceleration jumps at speed change points) can be selected; if the requirement is high smoothness (e.g., precision assembly, minimally invasive surgery scenario), an S-shaped curve (achieving continuous and smooth transition of speed and acceleration through jerk control); some systems can also support users to manually specify the curve type to adapt to special working conditions.

[0049] For example, the parameter configuration subunit can read the hardware parameters of the motor driver (such as maximum rated speed, maximum allowable acceleration, and maximum jerk) as the upper limit for parameter configuration, and then allocate parameters for each motion stage according to the total displacement and curve type. Taking a trapezoidal curve as an example, it is necessary to configure the acceleration value of the acceleration stage (not exceeding the upper limit), the maximum speed of the constant speed stage (not exceeding the upper limit), and the deceleration value of the deceleration stage (usually symmetrical to the acceleration). The displacement ratio of the acceleration, constant speed, and deceleration stages is deduced from the total displacement, thereby determining the duration ratio of each stage. For an S-shaped curve, the jerk value of the acceleration stage can also be configured to ensure that the acceleration smoothly increases from 0 to the maximum value and then decreases to 0. The duration ratio of each stage must meet the matching relationship between the total displacement and time, thereby generating an executable parameter list.

[0050] For example, the curve generation subunit can divide the motion into stages based on the total displacement, curve type, and configuration parameters (e.g., a trapezoidal curve is divided into three segments: acceleration, uniform speed, and deceleration; an S-curve is divided into seven segments: acceleration-acceleration, uniform acceleration, deceleration, uniform speed, acceleration-deceleration, uniform deceleration, and deceleration-deceleration). Then, a mathematical expression for position and velocity is established for each stage (e.g., the velocity of the acceleration segment of the trapezoidal curve is a uniform acceleration function v = at, and the position is a quadratic function s = ...). 1 / 2at 2 The acceleration segment of the S-curve has a linearly increasing acceleration function a = jt, and a quadratic velocity function v = ... 1 / 2jt 2 The position is a cubic function s = 1 / 6jt 3 Finally, the curves are stitched together using the connection conditions of each stage (such as the final velocity equaling the initial velocity of the next stage) to ensure the overall position-velocity curve is continuous without abrupt changes, and the curve data is output in the form of a function expression or a discrete point set. In the mathematical expression of the acceleration segment of the trapezoidal curve, v represents the instantaneous velocity of that stage (the unit is usually mm / s or rad / s, depending on the type of joint motion), and a represents the acceleration of the acceleration segment (the unit is mm / s²). 2 or rad / s 2 , where is a constant value, t represents the motion time of this stage (in seconds); the position function s represents the instantaneous position within the acceleration segment (in mm or rad), its value is determined by both acceleration and time, reflecting the cumulative displacement from the starting point of motion to that moment. In the expression for the acceleration segment of the S-curve, a represents the instantaneous acceleration of this stage (in mm / s²). 2 or rad / s 2 j represents jerk (unit: mm / s²) 3 or rad / s 3 , where is a constant value describing the rate of change of acceleration, t represents the motion time of this stage (in seconds); the velocity function v represents the instantaneous velocity of the acceleration segment (in mm / s or rad / s), and the position function s represents the instantaneous position of this stage (in mm or rad). By introducing jerk j, the S-curve achieves linear growth of acceleration starting from 0, avoiding the abrupt acceleration problems of trapezoidal curves and making velocity and position changes smoother.

[0051] For example, when the rotary joint of the main finger of the minimally invasive surgical robot needs to move from its current position (0°) to the target position (90°), the sub-units of the first determination unit work together: the displacement calculation sub-unit determines the total displacement to be 90° by calculating the difference between 90° and 0°; the curve type selection sub-unit selects an S-curve as the segmented planning curve type, considering the extremely high requirements for motion smoothness in the surgical scenario (to avoid impact); and the parameter configuration sub-unit configures the maximum allowable speed to be 30° / s and the maximum allowable acceleration to be 15° / s, taking into account the limitations of motor performance. 2 The jerk is 5° / s 3 The duration of each of the seven stages—acceleration, uniform acceleration, deceleration, constant speed, acceleration / deceleration, uniform deceleration, and deceleration—is set to 10%, 20%, 10%, 20%, 10%, 20%, and 10%, respectively. Based on the total displacement of 90°, the S-curve type, and the aforementioned parameters, the curve generation sub-unit establishes corresponding mathematical expressions for position and velocity for each stage (e.g., acceleration a = 5t and velocity v = 2.5t in the acceleration segment). 2 Position s = (5 / 6)t 3 By using the connection conditions of each stage (such as the final velocity of the deceleration segment being equal to the initial velocity of the uniform velocity segment), the curves are spliced ​​together, ultimately generating a continuous and smooth position-velocity curve, ensuring that the velocity and acceleration changes smoothly and without abrupt changes during the joint's movement from 0° to 90°.

[0052] Optionally, in some embodiments, the system may further include an anti-saturation processing module for:

[0053] When the cumulative value of the integral term in the position loop or velocity loop exceeds a preset threshold, the integral separation strategy is activated.

[0054] The initial current command output by the speed loop is dynamically limited; the limiting range is adjusted according to the real-time torque characteristics of the motor.

[0055] For example, the anti-saturation processing module can monitor the cumulative value of the integral term of the PID controller in the position loop and speed loop in real time, and preset integral thresholds for both (e.g., the integral threshold for the position loop is set to 80% of the maximum output, and the integral threshold for the speed loop is set to 70%). When the cumulative value of the integral term is detected to exceed the corresponding threshold (e.g., the output of the position loop is close to saturation due to continuous integration of a large error), the accumulation of the integral term is immediately cut off, and only the proportional term and the derivative term are retained to participate in the control, so as to avoid overshoot or response delay caused by excessive integration. When the system error decreases and the cumulative value of the integral term falls back to within the threshold, the integral term is automatically restored, and the steady-state error is eliminated through the integral link, so as to achieve a balance of "fast response when there is a large error and precise adjustment when there is a small error".

[0056] For example, when the main finger joint of the minimally invasive surgical robot is rotating rapidly, if the position loop deviates significantly from the target position, causing the cumulative value of the integral term to exceed a preset threshold (e.g., the threshold is set to 100 units), the anti-saturation processing module can immediately activate the integral separation strategy: temporarily stop the accumulation of the integral term, and only retain the proportional and differential terms for adjustment to avoid overshoot or response lag caused by integral saturation. After the deviation decreases and the cumulative value of the integral term falls back to within the threshold, the function of the integral term is restored.

[0057] For example, the anti-saturation processing module can also collect operating parameters such as current and speed in real time through the motor driver, and construct a real-time torque characteristic model by combining the motor nameplate parameters (such as rated torque and maximum torque curve) (or call the pre-stored torque-speed characteristic table); calculate the maximum allowable current under the current operating condition based on the model (e.g., when the torque output capability is strong at low speed, the limit can be relaxed to 120% of the rated current; when the torque decreases at high speed, the limit is tightened to 90% of the rated current); then compare the initial current command output by the speed loop with the dynamically calculated upper and lower limits. If the command exceeds the upper limit, it is clamped to the maximum value; if it is below the lower limit, it is clamped to the minimum value, ensuring that the output current is always within the safe and efficient torque output range of the motor, avoiding motor overheating or insufficient torque due to current over-limit.

[0058] For example, assuming the motor's current torque characteristics are non-linear due to load changes (e.g., the maximum safe current is 2A at low speeds and drops to 1.5A at high speeds), the anti-saturation processing module can collect the motor speed and torque feedback signals in real time and adjust the limiting range from a fixed value to a range that dynamically changes with the speed (e.g., a limit of 1.8A at 100° / s and a limit of 1.5A at 200° / s). This ensures that the output current command is always within the motor's safe operating range, thus avoiding excessive current that could burn out the motor and fully utilizing the motor's torque performance.

[0059] It is not difficult to see that in this embodiment, the speed planning module works collaboratively with the first determining unit and the second determining unit. Since the first determining unit plans a smooth position-velocity curve based on the current position and the target position, it can limit the speed and acceleration during motion to a reasonable range, avoiding shocks or overshoots caused by sudden trajectory changes, thus providing a stable motion reference for the system. The second determining unit can extract the planned position and planned speed from the position-velocity curve in real time, providing continuous and accurate reference input for the position loop and speed loop, ensuring that closed-loop adjustment always revolves around the preset trajectory. Therefore, it can reduce control deviations caused by reference signal lag or distortion. The combination of these two units can ensure the smoothness of motor motion (especially suitable for scenarios with stringent precision requirements, such as minimally invasive surgical robots), and accelerate the system convergence speed through precise trajectory guidance. It also lays the foundation for subsequent anti-interference control, fundamentally improving the dynamic response and control accuracy of the entire cascade control system.

[0060] Third Embodiment

[0061] The third embodiment of this application relates to a cascade control system. The third embodiment is an improvement upon the first embodiment, specifically in that it provides a concrete implementation of a position loop.

[0062] Optionally, the position ring may specifically include: a deviation calculation unit, a command generation unit, and a speed adjustment unit;

[0063] The deviation calculation unit is used to determine the position deviation based on the planned position and the current position;

[0064] The instruction generation unit is used to perform PID calculations on the position deviation to determine the initial speed instruction;

[0065] The speed adjustment unit is used to determine the reference speed based on the preliminary speed command.

[0066] For example, the deviation calculation unit can receive the planned position (a trajectory point dynamically updated over time) output by the second determination unit in real time, and simultaneously acquire the current position of the controlled motor (a digital quantity processed by the signal conditioning circuit) through a motor encoder or position sensor. Then, it converts the planned position and the current position to the same coordinate system (e.g., unifying them into angles or displacements), calculates the difference between the two through subtraction, and obtains the signed position deviation (a positive sign indicates the current position lags behind the planned position, and a negative sign indicates it leads). In some examples, to avoid deviation jitter caused by sensor noise, some systems can perform a first-order low-pass filter on the position deviation (e.g., a cutoff frequency of 50Hz), finally outputting a smooth position deviation value to the instruction generation unit.

[0067] For example, the instruction generation unit can read preset position loop PID parameters (proportional coefficient Kp, integral time Ti, and derivative time Td); and perform sub-term calculations on the position deviation output by the deviation calculation unit. Specifically, the proportional term (Kp × position deviation) can quickly respond to the current deviation and provide immediate adjustment force; the integral term (Kp / Ti × ∫ position deviation dt) can accumulate historical deviations and gradually eliminate steady-state errors (such as positioning errors caused by mechanical backlash); the derivative term (Kp × Td × d(position deviation) / dt) can predict the trend based on the rate of change of deviation and suppress overshoot; then, the results of the three calculations are superimposed to generate a preliminary speed instruction (the unit is usually ° / s or mm / s), which directly reflects the speed adjustment amount required to eliminate the position deviation.

[0068] For example, the speed adjustment unit can filter the current speed of the motor (calculated by a speed sensor or position derivative) (e.g., using a moving average filter or Kalman filter) to remove high-frequency noise; simultaneously, it can separately filter the derivative term (containing high-frequency disturbance information) output by the command generation unit (e.g., using a second-order Butterworth low-pass filter) to retain trend changes. Furthermore, the filtered current speed can be used as feedback and compared with the initial speed command. If a deviation exists, it can be corrected through proportional adjustment (e.g., reducing the command value if the current speed is too high). Then, combined with the filtered derivative term, damping compensation is applied to the corrected command (e.g., appropriately reducing the command when the derivative term is positive to suppress potential overshoot). Finally, a smooth and stable reference speed is output, ensuring that the command received by the speed loop accurately tracks the position requirements while avoiding speed fluctuations caused by instantaneous disturbances.

[0069] For example, when the planned position of the main finger joint of the minimally invasive surgical robot is 30° and the current position is 25°, the deviation calculation unit of the position loop can calculate a position deviation of 5°. The command generation unit performs PID calculations on this 5° deviation, assuming the proportional term outputs 3° / s, the integral term outputs 0.5° / s, and the derivative term outputs 0.3° / s, and sums them to obtain an initial speed command of 3.8° / s. The speed adjustment unit can then introduce the current speed after low-pass filtering (e.g., the actual current speed is 2.2° / s, and after filtering it is 2.1° / s) and the filtered derivative term (e.g., the original derivative term is 0.3° / s, and after filtering it is 0.25° / s) to correct the initial speed command (e.g., if the current speed is slightly lower than expected, fine-tune by increasing it by 0.1° / s), determine the reference speed as 3.9° / s, and output it to the speed loop. In this way, the responsiveness to position deviations can be guaranteed, and motion oscillations caused by sudden changes in commands can be avoided through filtering and dynamic adjustment.

[0070] It should be noted that this embodiment can also be an improvement based on the second embodiment.

[0071] It is not difficult to see that in this embodiment, the position loop, through the synergistic effect of the deviation calculation unit, the command generation unit, and the speed adjustment unit, can significantly improve control accuracy and stability. Specifically, the deviation calculation unit obtains the position deviation between the planned position and the current position, which can provide an accurate error benchmark for subsequent adjustments and avoid control deviation caused by deviation calculation distortion; the command generation unit performs PID calculation on the position deviation, using the proportional term to quickly respond to the deviation, the integral term to eliminate steady-state error, and the derivative term to suppress overshoot, generating an initial speed command with basic adjustment capability; the speed adjustment unit introduces the filtered current speed (filtering out high-frequency noise and avoiding interference) and the filtered derivative term (smoothing the derivative signal and reducing oscillation) to dynamically correct the initial speed command, making the reference speed more consistent with the actual operating state of the motor. The combination of these three components can ensure the system's rapid response and steady-state accuracy to position deviation through PID calculation, and weaken the influence of noise and signal abrupt changes through filtering and dynamic adjustment, making the output reference speed more accurate and smooth, laying a reliable foundation for further adjustment of the speed loop, and improving the anti-interference capability and dynamic performance of the entire cascade control system.

[0072] Fourth embodiment

[0073] The fourth embodiment of this application relates to a cascade control system. The fourth embodiment is an improvement upon the first embodiment, specifically in that it provides a concrete implementation of a speed loop.

[0074] Optionally, the velocity loop may specifically include:

[0075] A target velocity synthesis unit is used to determine a target reference velocity based on the planned velocity and the reference velocity.

[0076] A current command generation unit is used to determine a reference current based on the target reference speed and the current speed.

[0077] For example, the target velocity synthesis unit, after receiving the planned velocity (reflecting the basic velocity of the preset trajectory) output by the velocity planning module and the reference velocity (reflecting the compensation velocity of position adjustment) output by the position loop, can process the two through a preset synthesis algorithm. For example, a linear superposition method can be used (i.e., target reference velocity = planned velocity + reference velocity). In some application scenarios, weighting coefficients (such as a weight of 0.7 for the planned velocity and a weight of 0.3 for the reference velocity) can be introduced to balance the priority of trajectory tracking and deviation correction. Finally, the synthesized target reference velocity is output, which can retain the basic characteristics of the original planned trajectory and incorporate the real-time adjustment compensation of the position loop, making the velocity target more consistent with the current motion state.

[0078] Optionally, in some embodiments, the current command generation unit is specifically used to: determine a speed deviation based on the target reference speed and the current speed; execute a PID algorithm on the speed deviation to obtain a preliminary current command; and determine a reference current based on the preliminary current command.

[0079] For example, the current command generation unit can calculate the difference between the target reference speed and the current speed of the motor (acquired in real time by the encoder and filtered) to obtain the speed deviation; then, it performs PID calculation on the speed deviation. Specifically, the proportional term (P) directly outputs the adjustment amount according to the magnitude of the deviation, quickly responding to the current error; the integral term (I) accumulates historical deviations to eliminate steady-state errors; the derivative term (D) predicts the trend of deviation changes and suppresses overshoot; finally, the PID calculation result is converted into a reference current command (the speed adjustment amount is converted into the corresponding current value through the motor torque constant), and output to the subsequent anti-saturation processing module or directly transmitted to the current loop of the driver, realizing the conversion from speed regulation to current output.

[0080] For example, when the main finger joint of a minimally invasive surgical robot needs to move at a planned speed of 5° / s, and the reference speed output by the position loop is 4.8° / s, the target speed synthesis unit of the speed loop will weight and fuse these two speeds (e.g., with the planned speed accounting for 60% and the reference speed accounting for 40%) to calculate the target reference speed as (5×0.6+4.8×0.4)=4.92° / s. This retains the baseline of the planned trajectory while incorporating the real-time adjustment of the position loop. Subsequently, the current command generation unit compares the target reference speed of 4.92° / s with the current motor speed of 4.7° / s, obtaining a speed deviation of 0.22° / s. This deviation is then converted into a corresponding reference current (e.g., 0.8A) through PID calculation (e.g., proportional coefficient amplifies the deviation, integral term eliminates accumulated error). This current value is transmitted to the current loop of the driver as the basis for motor acceleration, ensuring that the joint speed quickly approaches the target value.

[0081] It should be noted that this embodiment may also be an improvement based on the second and / or third embodiments.

[0082] It is easy to see that in this embodiment, the target speed synthesis unit merges the planned speed (reflecting the trajectory baseline) with the reference speed output by the position loop (reflecting position adjustment requirements) into a target reference speed. This ensures the consistency of the motion trajectory and allows for real-time response to adjustment needs caused by position deviations, avoiding trajectory deviations or adjustment lags that may result from a single speed command. The current command generation unit can adjust based on the deviation between the target reference speed and the current speed, converting the speed deviation into a reference current to directly drive the motor to output a matching torque, achieving rapid speed convergence. The combination of these two methods balances trajectory planning and real-time adjustment through speed synthesis, while ensuring the immediacy and accuracy of speed control through precise current command generation. This improves the system's tracking accuracy of speed commands and its resistance to load disturbances.

[0083] Fifth Embodiment

[0084] The fifth embodiment of this application relates to a cascade control system. The fifth embodiment is an improvement upon the first embodiment, specifically in that the control module may further include a filtering module.

[0085] Specifically, the filtering module may include:

[0086] The speed filtering unit is used to perform low-pass filtering or Kalman filtering on the current speed of the input speed loop;

[0087] The differential filtering unit is used to perform low-pass filtering on the differential term calculation in the position loop PID operation.

[0088] In other words, in this embodiment, the cascaded PID control part also involves filtering and control cycle adjustment. Filtering can include speed filtering and derivative filtering. Speed ​​filtering is applied to the current speed of the speed PID input, and the methods include low-pass filtering and Kalman filtering. Derivative filtering is applied to the derivative calculation in the PID, and belongs to low-pass filtering.

[0089] For example, the speed filtering unit can receive the raw current speed signal acquired from a motor sensor (such as an encoder). This signal may contain high-frequency noise caused by mechanical vibration and electromagnetic interference. Then, based on the system's requirements for response speed and noise suppression, it selects either a low-pass filter or a Kalman filter algorithm. If a low-pass filter is used, noise components higher than this frequency can be filtered out by setting an appropriate cutoff frequency (e.g., 50Hz), retaining only the low-frequency signal reflecting the actual motion state of the motor. If a Kalman filter is used, a dynamic model of the motor speed can be established, and recursive calculations can be performed by combining prior estimates with current measurements to suppress noise while preserving the dynamic change characteristics of the speed. Thus, a smoothed, filtered speed signal can be output to the speed loop.

[0090] For example, the differential filtering unit can acquire the rate of change of position deviation (original differential term). This signal may generate high-frequency oscillations due to small fluctuations in position sampling. It is then processed by a first-order low-pass filter (such as an RC filter circuit or a digital filtering algorithm), setting a filtering time constant (e.g., 0.01 seconds) to attenuate high-frequency components and retain the effective adjustment component of the differential term. By having the processed differential term, proportional term, and integral term participate in the calculation in tandem, the overshoot suppression effect of the differential term can be maintained, and system oscillations caused by noise in the original differential term can be avoided, thus improving the stability of the position loop output command.

[0091] For example, when the main finger joint of the minimally invasive surgical robot moves, the speed filtering unit can process the current speed signal collected by the motor encoder: if the original speed signal exhibits high-frequency fluctuations above 100Hz due to gear meshing vibration (e.g., the actual speed is stable at 5° / s, while the original signal oscillates at high frequencies between 4.5° / s and 5.5° / s), a low-pass filter with a cutoff frequency of 50Hz can be used to smooth the output speed signal to a stable value of 4.9° / s to 5.1° / s, thereby effectively eliminating high-frequency noise; simultaneously, the differential filtering unit can address the jitter of the differential term in the position loop caused by position sampling errors (e.g., the original differential term fluctuates at 0.2° / s). 2 up to 0.8° / s 2 (The temperature fluctuates drastically), and is stabilized at 0.4° / s by low-pass filtering with a time constant of 0.02 seconds. 2 up to 0.6° / s 2 The reasonable range is such that the function of suppressing overshoot by the differential term can be retained, while speed command fluctuations caused by signal jitter can be avoided.

[0092] It should be noted that this embodiment may also be an improvement based on any one or more embodiments of the second to fourth embodiments.

[0093] It is easy to see that in this embodiment, by using a speed filtering unit to perform low-pass filtering or Kalman filtering on the current speed of the input speed loop, high-frequency interference such as sensor noise and mechanical vibration can be effectively filtered out, avoiding speed signal distortion caused by noise. This allows the speed loop to obtain a more reliable feedback reference and reduces erroneous adjustments caused by false speed signals. By using a differential filtering unit to perform low-pass filtering on the differential term of the position loop PID, the excessive sensitivity of the differential term to small position fluctuations can be suppressed, eliminating high-frequency oscillations that are prone to occur in the original differential term. This ensures that the differential term suppresses overshoot while avoiding damage to system stability. The combination of these two methods not only achieves the purpose of purifying the speed feedback signal but also optimizes the adjustment characteristics of the position loop, reducing noise interference to control commands from the source. This makes the adjustment of each link in the cascade PID more closely match the actual operating state of the motor, which is beneficial to improving the anti-interference capability and motion smoothness of the entire control system.

[0094] Sixth Embodiment

[0095] The sixth embodiment of this application relates to a cascade control system. The sixth embodiment is an improvement upon the first embodiment, specifically in that the control module further includes a period adjustment module for hierarchical configuration of the control period.

[0096] Specifically, the control cycle of the position loop is 2-5 times the control cycle of the speed loop;

[0097] Specifically, the control cycle of the speed loop is greater than the sum of the current loop stabilization time and the cross-layer communication delay;

[0098] Specifically, the execution frequency of the current loop is higher than the control frequency of the speed loop.

[0099] For example, in this embodiment, the position loop, as the outer control layer, is mainly responsible for long-cycle position trajectory tracking and has relatively low real-time requirements; while the speed loop, as the inner control layer, needs to be adjusted more frequently to respond to speed deviations. Since both operate within the controller and do not require cross-device communication, using a cycle ratio of 2-5 times (e.g., when the speed loop cycle is 1ms, the position loop cycle is set to 2-5ms) is an empirically verified value. This ensures that the position loop has sufficient time to process trajectory planning information and allows the speed loop to quickly respond to changes in the position loop's commands, avoiding control lag or resource waste caused by improper cycle matching, thereby balancing the system's computational load and control accuracy.

[0100] For example, in this embodiment, considering that the output (current command) of the speed loop needs to be transmitted from the controller to the current loop of the driver, this process involves communication delay; at the same time, it also takes a certain amount of time for the current loop to complete an adjustment and reach a stable state. If the speed loop period is less than the sum of these two (for example, the current loop needs 0.5ms to stabilize, the communication delay is 0.3ms, the total is 0.8ms, while the speed loop period is set to 0.6ms), then the current loop has not yet fully responded to the previous command, and a new command has already been generated, which will lead to command superposition interference and damage the system stability. Therefore, by designing the speed loop period to be greater than this total (e.g., set to 1ms), it can be ensured that the current loop has sufficient time to complete the adjustment and provide feedback, so that each adjustment of the speed loop is based on the stability of the current loop, ensuring the effectiveness of the control command.

[0101] For example, in this embodiment, considering that the current loop directly controls the input current of the motor and needs a fast response to suppress current fluctuations, its execution frequency (e.g., 10kHz) needs to be high enough to accurately track the current command and ensure stable torque output from the motor. The speed loop, on the other hand, controls speed by adjusting the current command, and its control frequency (e.g., 1kHz) can be relatively low because speed changes are the result of torque accumulation and do not require high-frequency adjustment like current. This frequency hierarchy (current loop > speed loop) conforms to the control logic of "high frequency in the inner layer, low frequency in the outer layer," allowing the current loop to quickly eliminate underlying disturbances and the speed loop to adjust speed based on a relatively stable current, thus avoiding the resource consumption caused by high-frequency control. It also ensures a smooth and efficient adjustment link from current to speed in the system.

[0102] It should be noted that this embodiment may also be an improvement based on any one or more embodiments from the second to the fifth embodiment.

[0103] It is not difficult to see that in this embodiment, by setting the position loop control cycle to 2-5 times that of the speed loop, the functional division of the outer layer (position loop) which focuses on trajectory planning and has lower real-time requirements, and the inner layer (speed loop) which focuses on rapid adjustment, is not only consistent with this, but also balances the controller's computing resources through a reasonable cycle ratio, avoiding the load pressure caused by high-frequency computing. Because the speed loop control cycle is greater than the sum of the current loop's settling time and the cross-layer communication delay, it ensures that after the current command output by the speed loop is transmitted to the driver, the current loop has sufficient time to complete adjustment and reach a stable state, thereby avoiding disturbances caused by command superposition and ensuring the effectiveness of cross-layer control. Furthermore, because the current loop's execution frequency is higher than the speed loop's control frequency, the current loop can respond quickly and suppress the bottom current fluctuations, providing a stable torque base for the speed loop. The combination of these three factors, through the hierarchical cycle design of "outer layer low-frequency planning, middle layer medium-frequency adjustment, and inner layer high-frequency execution," not only meets the functional requirements of different control loops, but also ensures the coordination between each layer, which is beneficial to improving the system's stability, response speed, and anti-interference capability.

[0104] It is worth mentioning that all modules involved in this embodiment are logical modules. In practical applications, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. Furthermore, to highlight the innovative aspects of this application, this embodiment does not introduce units that are not closely related to solving the technical problems proposed in this application; however, this does not mean that other units are absent in this embodiment.

[0105] Seventh Embodiment

[0106] The seventh embodiment of this application relates to a cascade control method, which can be applied to a system as described in any one or more embodiments from the first to the seventh embodiment, such as... Figure 2 As shown, the method may include:

[0107] Step S101: Determine the planned location and planned speed;

[0108] Step S102: Output a reference speed based on the planned position;

[0109] Step S103: Determine the reference current based on the reference speed and the planned speed;

[0110] Step S104: Output the reference current to the controlled motor.

[0111] It is not difficult to see that this embodiment is a method embodiment corresponding to the first embodiment, and this embodiment can be implemented in conjunction with the first embodiment. The relevant technical details mentioned in the first embodiment are still valid in this embodiment, and will not be repeated here to reduce repetition. Accordingly, the relevant technical details mentioned in this embodiment can also be applied to the first embodiment.

[0112] The steps of the various methods described above are only for clarity. In practice, they can be combined into one step or some steps can be split into multiple steps. As long as they include the same logical relationship, they are all within the scope of protection of this application. Adding insignificant modifications or introducing insignificant designs to the algorithm or process, but without changing the core design of the algorithm and process, are also within the scope of protection of this application.

[0113] Furthermore, some embodiments of this application also provide a minimally invasive surgical robot. The minimally invasive surgical robot can be various forms of digital computers, such as laptops, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, etc. The minimally invasive surgical robot can also be various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices.

[0114] The minimally invasive surgical robot includes: one or more processors; and a memory storing computer program instructions, which, when executed, cause the processors to perform the steps of the methods provided in any one or more of the above embodiments. Figure 3 An exemplary structural diagram of the minimally invasive surgical robot is disclosed, including: one or more processors 1101, a memory 1102, and interfaces for connecting the various components, including high-speed interfaces and low-speed interfaces. The various components are interconnected using different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the minimally invasive surgical robot, including instructions stored in or on memory to display graphical information of a GUI on an external input / output device (such as a display device coupled to the interface). In some other embodiments, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple minimally invasive surgical robots can be connected, each device providing some of the necessary operations. The components, their connections and relationships, and their functions shown herein are merely examples and are not intended to limit the implementation of the present application described and / or claimed herein.

[0115] The minimally invasive surgical robot may further include an input device 1103 and an output device 1104. The processor 1101, memory 1102, input device 1103 and output device 1104 may be connected by a bus or other means, as shown in the figure, which is connected by a bus.

[0116] Input device 1103 can receive input numerical or character information, and generate key signal inputs related to user settings and function control of the minimally invasive surgical robot, such as a touchscreen, keypad, mouse, trackpad, touchpad, pointer, one or more mouse buttons, trackball, joystick, etc. Output device 1104 may include a display device, auxiliary lighting device (e.g., LED), and haptic feedback device (e.g., vibration motor). The display device may include, but is not limited to, a liquid crystal display, a light-emitting diode display, and a plasma display. In some embodiments, the display device may be a touchscreen.

[0117] To provide interaction with the user, the minimally invasive surgical robot can be a computer. This computer has: a display device (e.g., a cathode ray tube or LCD monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback); and input from the user can be received in any form (e.g., voice input or tactile input).

[0118] The flowcharts or block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of devices, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, may be implemented using a dedicated hardware-specific system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0119] The scope of this application is defined by the appended claims rather than the foregoing description, and is therefore intended to encompass all variations falling within the meaning and scope of equivalents of the claims. No reference numerals in the claims should be construed as limiting the scope of the claims. Furthermore, it is clear that the word "comprising" does not exclude other units or steps, and the singular does not exclude the plural. Multiple units or devices recited in a device claim may also be implemented by a single unit or device in software or hardware. Terms such as "first," "second," etc., are used only for distinguishing descriptions and do not indicate any particular order, nor should they be construed as indicating or implying relative importance.

[0120] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily made by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims, and the above embodiments should be regarded as exemplary and non-limiting.

Claims

1. A cascade control system, the system comprising a controller and a driver, characterized in that, The controller includes a speed planning module and a control module; the control module includes a position loop and a speed loop; the driver includes a current loop; The speed planning module is used to determine the planned location and planned speed; The position loop is used to output a reference speed to the speed loop based on the planned position; The speed loop is used to determine a reference current based on the reference speed and the planned speed, and to transmit the reference current to the driver; The current loop is used to output current to the controlled motor according to the reference current.

2. The system according to claim 1, characterized in that, The speed planning module specifically includes a first determining unit and a second determining unit: The first determining unit is used to determine the position-velocity curve based on the current position and the target position of the controlled motor; The second determining unit is used to determine the planned position and planned speed based on the position-velocity curve.

3. The system according to claim 2, characterized in that, The first determining unit includes: The displacement calculation subunit is used to determine the total displacement based on the difference between the current position and the target position of the controlled motor. The curve type selection sub-unit is used to determine the type of piecewise planning curve based on the system's requirements for motion smoothness and response speed. The parameter configuration subunit is used to configure the motion parameters corresponding to the segmented planning curve type, including the maximum allowable speed, the maximum allowable acceleration, the jerk, and the duration percentage of each motion stage. The curve generation sub-unit is used to determine the position-velocity curve based on the total displacement, the segmented planning curve type, and the motion parameters.

4. The system according to claim 1, characterized in that, The position loop specifically includes: a deviation calculation unit, a command generation unit, and a speed adjustment unit; The deviation calculation unit is used to determine the position deviation based on the planned position and the current position; The instruction generation unit is used to perform PID calculations on the position deviation to determine the initial speed instruction; The speed adjustment unit is used to determine the reference speed based on the preliminary speed command.

5. The system according to claim 1, characterized in that, The velocity loop specifically includes: A target velocity synthesis unit is used to determine a target reference velocity based on the planned velocity and the reference velocity. A current command generation unit is used to determine a reference current based on the target reference speed and the current speed.

6. The system according to claim 5, characterized in that, The current command generation unit is specifically used for: Determine the speed deviation based on the target reference speed and the current speed; A PID algorithm is applied to the speed deviation to obtain an initial current command; Determine the reference current based on the initial current command.

7. The system according to claim 1, characterized in that, The control module further includes a filtering module; the filtering module includes: The speed filtering unit is used to perform low-pass filtering or Kalman filtering on the current speed of the input speed loop; The differential filtering unit is used to perform low-pass filtering on the differential term calculation in the position loop PID operation.

8. The system according to claim 1, characterized in that, The control module further includes a period adjustment module for hierarchical configuration of the control period; wherein: The control cycle of the position loop is 2-5 times the control cycle of the speed loop; The control cycle of the speed loop is greater than the sum of the current loop settling time and the cross-layer communication delay. The execution frequency of the current loop is higher than the control frequency of the speed loop.

9. A cascade control method, characterized in that, The method is applied to the system as described in any one of claims 1-8, and the method includes: Determine the planning location and speed; Based on the planned location, a reference speed is output to the speed loop; A reference current is determined based on the reference speed and the planned speed, and the reference current is transmitted to the driver; The output current is sent to the controlled motor according to the reference current.

10. A minimally invasive surgical robot, characterized in that, The minimally invasive surgical robot includes: One or more processors; and A memory storing computer program instructions, which, when executed, cause the processor to perform the steps of the method as described in claim 9.

Images