CN118372255B - Human-machine safety enhanced shared control method and device integrating position, speed and force - Google Patents

Abstract

The invention provides a man-machine safety enhancement sharing control method and device integrating position, speed and force, and relates to the field of automation and artificial intelligence. The method provided by the invention integrates three target controllers of master hand position tracking, master hand speed tracking and robot planning track tracking, and ensures the continuous control capability of a surgeon in the whole operation process, thereby enhancing the safety of human-computer interaction; a target prediction evaluation mechanism based on the current motion state is established, and intelligent dynamic allocation of control authorities among three controllers can be realized; a force feedback mechanism is designed to help the surgeon identify the difference between the real-time position of the master cell phone robot and the motion fusion control output, and to enhance the operational intuition of the surgeon to enhance the safety of the shared control.

Description

Human-machine safety enhanced shared control method and device integrating position, speed and force

Technical Field

The present invention relates to the field of automation and artificial intelligence, and in particular to a method and device for enhancing human-machine safety shared control by integrating position, speed and force.

Background Art

With the rapid development of automation and artificial intelligence, robots have been widely infiltrated into people's daily life and work. The cooperation mode between humans and robots has also changed from the traditional coexistence without interaction to the current control mode of remote control, human-machine collaboration, and human-machine sharing and cooperation. At present, it is difficult for robots to perform tasks completely autonomously in the face of unstructured scenes or complex tasks. The human-machine interaction method can combine the advantages of both parties: humans have the ability to reason and solve problems, and can adapt flexibly to complex environments; robots have the advantages of anti-fatigue and high precision in execution, and the special material structure characteristics can also be competent for harsh working environments.

Existing research on human-machine shared control mainly focuses on improving the performance and efficiency of task execution, autonomous and intelligent robot correction of human behavior, and active adaptation of robots to human behavior. Related applications include retaining the driver's control ability under autonomous driving, virtual fixtures providing guidance and collision avoidance, reinforcement learning to identify and adapt to human behavior, autonomous tremor compensation to improve surgical quality, and intelligent role switching in production collaboration to improve production efficiency.

For example, A Confidence-Based Shared Control Strategy for the Smart Tissue Autonomous Robot (STAR) proposes a confidence-based control rights allocation function that can automatically switch between autonomous control and manual control, thereby improving the overall task performance of the robot-assisted surgery system. However, although this scheme considers the combination of robot autonomous control and human control, it overemphasizes the use of autonomous intelligence and ignores the dominant role of humans in surgery. In the process of autonomous intelligence as a leader, when the surgeon is aware of the impending danger, the operator lacks situational awareness when returning to the control loop, and the sudden switch from autonomous control to manual control will lead to an increase in transient errors, which will increase the risk of the system if not handled properly.

Another example is "Adaptive Impedance Controller for Human-Robot Arbitration based on Cooperative Differential Game Theory", which proposes a human-robot interaction control weight distribution method based on cooperative differential game to solve the role switching problem between humans and machines during physical human-robot interaction. However, although this solution can collect the force applied by the person at the main hand end through the force sensor to achieve the purpose of estimating the person's control intention, considering that the hand feel is very important for the doctor's control during the operation, in the high-frequency interaction between the robot's autonomous control strategy and the doctor's surgical guidance intention, there is a lack of force feedback that conforms to the doctor's control intuition, and it is difficult to ensure the safety and controllability of the surgical execution process.

Summary of the invention

1. Technical issues to be resolved

In view of the shortcomings of the prior art, the present invention provides a human-machine safety enhanced shared control method and device that integrates position, speed and force, which solves the technical problems that the complete transfer of roles between humans and robots poses a hidden danger that doctors cannot control, and the lack of mechanical perception information cannot effectively ensure the safety of robotic surgery.

(II) Technical solution

To achieve the above objectives, the present invention is implemented through the following technical solutions:

A human-machine safety enhanced shared control method integrating position, speed and force, comprising:

Simplify the robot system into a spring-mass-damper system and establish the state equation;

According to the target requirements of human-machine shared control, three target controllers are set, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking;

Establish the corresponding motion predictive control model to obtain the predictive control state of each target controller;

According to each predictive control state, a penalty mechanism is introduced to set the evaluation function of each target controller;

According to each evaluation function, the predicted evaluation gradient descent value of each control target is calculated to obtain the authority allocation vector of each target controller in different scenarios;

The authority allocation vector is used as the input of the state equation to output the fused system motion state and motion speed;

According to the fused system motion state and the real-time position of the master-hand robot, a tactile force acting on the master-hand robot is generated based on a force feedback mechanism;

The rotation speed of the joint angle of the slave hand robot is generated according to the fused motion speed and the speed of posture change.

Preferably, the state equation is:

Among them, x is the system motion state, is the system motion speed, is the acceleration of the system, u is the total control input of the system, y is the total control output of the system, m , b and k are the inertia parameter, damping parameter and stiffness parameter of the spring-mass-damper system respectively.

Preferably, according to the target requirements of human-machine shared control, three target controllers are set respectively, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking; including:

Set up the main hand position tracking controller:

Among them, u _hx is the input of the master position tracking controller, k _px and k _dx are the proportional coefficient and differential coefficient of the master position tracking controller, x _hd is the real-time position of the master robot, e _{hx is} the deviation between the real-time position of the master robot and the system motion state, The speed of change of the deviation between the real-time position of the master robot and the system motion state;

Set up the master hand velocity tracking controller:

Among them, u _hv is the input of the master hand speed tracking controller, k _pv is the proportional coefficient of the master hand speed tracking controller, v _hd is the real-time speed of the master hand robot, and e _{hv is} the deviation between the real-time speed of the master hand robot and the system motion speed;

Set up the planning trajectory tracking controller:

Among them, urx is _the input of the planned trajectory tracking controller, kpr and kdr are the proportional coefficient and differential coefficient of the planned trajectory tracking controller, xrd is _the real-time position of the planned trajectory, _{and erx} is _the deviation between the real-time position of the planned trajectory and the system motion state. It is the speed of change of the deviation between the real-time position of the planned trajectory and the system motion state.

Preferably, the establishment of a corresponding motion prediction control model to obtain the prediction control state of each target controller includes:

in, is the predicted motion state of controller i within the prediction interval, is the predicted motion speed of controller i within the prediction interval, is the predicted acceleration of controller i within the prediction interval, is the predicted control output of controller i within the prediction interval;

When i=1 , the corresponding predictive control state of the master hand position tracking controller is ;

When i=2 , the corresponding predictive control state of the master hand speed tracking controller is ;

When i=3 , the corresponding predicted control state of the robot planning trajectory tracking controller is .

Preferably, the step of introducing a penalty mechanism to set the evaluation function of each target controller according to each predictive control state includes:

Establish the target evaluation function for the master hand position tracking controller:

Among them, J _hx is the cumulative value of the objective function of the master hand position tracking controller in the prediction interval from t ₀ to t ₀ +T , G _hx is the target evaluation function value of the master hand position tracking controller at a single time point in the prediction interval, and x ₀ is the motion state of the system at time t ₀ ; is the predicted control position of the master position tracking control target, which is calculated by rolling forward average according to the current state; P _hx is the penalty function, v ₀ is the system control speed threshold that triggers the penalty, and is used to When J _hx is reduced;

Establish the target evaluation function for the master hand speed tracking controller:

Among them, J _hv is the cumulative value of the objective function of the master hand speed tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _{hv is} the target evaluation function value of the master hand speed tracking controller at a single time point in the prediction interval; The predicted control speed of the master hand speed tracking control target is calculated by rolling forward average according to the current state; The predicted control state speed of the master hand speed tracking controller;

Establish the target evaluation function for the robot planning trajectory tracking controller:

Among them, J _rx is the cumulative value of the objective function of the planned trajectory tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _rx is the target evaluation function value of the planned trajectory tracking controller at a single time point in the prediction interval; is the predicted control position of the planned trajectory tracking control target, which is calculated by rolling forward average _of the current state; d0 is the distance threshold for triggering penalty, Prx is the position distance penalty function, which is used When J _rx is reduced; Prv is the speed distance penalty function, used When J _rx is reduced.

Preferably, the method of calculating the predicted evaluation gradient descent value of each control target according to each evaluation function to obtain the authority allocation vector of each target controller in different scenarios includes:

The predicted evaluation gradient descent values are calculated for the three control objectives of master hand position tracking, master hand velocity tracking, and robot planning trajectory tracking:

in, is the predicted evaluation gradient descent value of the main hand position tracking control target, is the predicted evaluation gradient descent value of the main hand speed tracking control target, Predict and evaluate the gradient descent value of the robot's trajectory tracking control target;

According to the scene control requirements, it is integrated into the global system input to obtain the permission allocation vector of each target controller in different scenes:

Among them, Ko _is the proportional coefficient matrix, λ is the step size of gradient adjustment, ∇J is the total prediction evaluation gradient descent value, φα is the saturation suppression function, ε is a very small constant, and α _is the threshold constant.

Preferably, the method of generating a tactile force acting on the master-hand robot based on a force feedback mechanism according to the fused system motion state and the real-time position of the master-hand robot comprises:

in, is the motion state of the fused system, F is the tactile force fed back to the master hand robot, are the error, error change speed and error change acceleration between the fused system motion state and the real-time position of the master hand robot _, respectively. kf , bf _and mf represent the stiffness parameter, damping parameter and inertia parameter of the force feedback system _, respectively.

Preferably, the method of generating the rotation speed of the joint angle of the slave hand robot according to the fused movement speed and the speed of posture change comprises:

in, is the fused motion speed, xm represents the fixed distal motion center, Xt represents the _{X -} axis vector of the posture matrix, Yt represents the Y _- axis vector of the posture matrix, Zt represents the Z-axis vector of the posture matrix, qt _represents the posture of the robot, and the function Indicates the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system. is the Euler angle in Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the joint angle of the slave robot; J ^-1 is the Jacobian matrix, are the joint angles of the slave robot.

A human-machine safety enhanced shared control device integrating position, speed and force, comprising:

Establish a module to simplify the robot system into a spring-mass-damper system and establish the state equation;

A setting module is used to set three target controllers, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking, according to the target requirements of human-machine shared control;

A prediction module is used to establish a corresponding motion prediction control model to obtain the prediction control state of each target controller;

A setting module is used to introduce a penalty mechanism to set the evaluation function of each target controller according to each predictive control state;

A calculation module is used to calculate the predicted evaluation gradient descent value of each control target according to each evaluation function, so as to obtain the authority allocation vector of each target controller in different scenarios;

An output module is used to use the authority allocation vector as the input of the state equation to output the fused system motion state and motion speed;

A feedback module is used to generate a tactile force acting on the master hand robot based on a force feedback mechanism according to the fused system motion state and the real-time position of the master hand robot;

The generation module is used to generate the rotation speed of the joint angle of the slave hand robot according to the fused movement speed and the speed of posture change.

(III) Beneficial effects

The present invention provides a method and device for enhancing human-machine safety shared control by integrating position, speed and force. Compared with the prior art, it has the following beneficial effects:

The present invention sets three controllers for master hand position tracking, master hand speed tracking and robot planned trajectory tracking to achieve position-speed fusion, thereby improving the human control ability in human-machine shared control; in addition, the fusion results of the three controllers are associated with the force feedback mechanism to achieve position-speed-force fusion, thereby enhancing the safety of system human-machine interaction.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

FIG1 is a block diagram of a method for enhancing human-machine safety sharing by integrating position, velocity and force provided by an embodiment of the present invention;

FIG2 is a schematic diagram of the application principle of a human-machine safety enhanced shared control method integrating position-velocity-force provided by an embodiment of the present invention in a hepatic lobectomy surgical scenario.

DETAILED DESCRIPTION

In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

The embodiments of the present application provide a human-machine safety-enhanced shared control method and device that integrates position, speed and force, thereby solving the technical problems that the complete transfer of roles between humans and robots poses a hidden danger that cannot be controlled by doctors, and that the lack of mechanical perception information cannot effectively ensure the safety of robotic surgery.

The technical solution in the embodiment of the present application is to solve the above technical problems, and the overall idea is as follows:

In view of the shortcomings of the prior art, the present invention provides a human-machine safety enhanced shared control method that integrates position, speed and force. In view of the shortcomings of the existing complete transfer of roles between humans and robots, such as the hidden dangers that cannot be controlled by doctors and the lack of mechanical perception information that cannot effectively ensure the safety of robotic surgery, a human-machine safety enhanced shared control method that integrates position, speed and force is constructed.

This method integrates three target controllers: master hand position tracking, master hand velocity tracking, and robot planning trajectory tracking, ensuring the surgeon's continuous control throughout the entire surgical process, thereby enhancing the safety of human-computer interaction; a target prediction and evaluation mechanism based on the current motion state is established, which can realize the intelligent dynamic allocation of control rights among the three controllers; a force feedback mechanism is designed to help surgeons identify the difference between the real-time position of the master hand robot and the motion fusion control output, thereby improving the surgeon's operational intuition and enhancing the safety of shared control.

In order to better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings and specific implementation methods.

Example:

As shown in FIG1 , an embodiment of the present invention provides a method for enhancing human-machine safety shared control by integrating position, velocity and force, including:

S1. Simplify the robot system into a spring-mass-damper system and establish the state equation;

S2. According to the target requirements of human-machine shared control, three target controllers are set: master hand position tracking, master hand speed tracking, and robot planning trajectory tracking;

S3, establishing a corresponding motion prediction control model to obtain the prediction control state of each target controller;

S4. According to each predictive control state, a penalty mechanism is introduced to set the evaluation function of each target controller;

S5. According to each evaluation function, the predicted evaluation gradient descent value of each control target is calculated to obtain the authority allocation vector of each target controller in different scenarios;

S6, using the authority allocation vector as the input of the state equation to output the fused system motion state and motion speed;

S7, generating a tactile force acting on the master-hand robot based on a force feedback mechanism according to the fused system motion state and the real-time position of the master-hand robot;

S8. Generate the rotation speed of the joint angle of the slave hand robot according to the fused movement speed and posture change speed.

In the above scheme, the embodiment of the present invention sets three controllers for master hand position tracking, master hand speed tracking and robot planned trajectory tracking to achieve position-speed fusion, thereby improving the human control ability in human-machine shared control; in addition, the fusion results of the three controllers are associated with the force feedback mechanism to achieve position-speed-force fusion, thereby enhancing the safety of human-machine interaction in the system.

The following are the steps of the above solution:

In step S1, the robot system is simplified into a spring-mass-damper system, and a state equation is established; wherein the state equation is:

Among them, x is the system motion state, is the system motion speed, is the acceleration of the system, u is the total control input of the system, y is the total control output of the system, m , b and k are the inertia parameter, damping parameter and stiffness parameter of the spring-mass-damper system respectively.

In step S2, according to the target requirements of human-machine shared control, three target controllers are set, namely, master hand position tracking, master hand speed tracking, and robot planning trajectory tracking; including:

(1) For the position tracking target of the main hand robot (directly controlled by the surgeon or other operator), set up the main hand position tracking controller to ensure that the robot can follow the cutting path of the main hand robot:

Among them, u _hx is the input of the master position tracking controller, k _px and k _dx are the proportional coefficient and differential coefficient of the master position tracking controller, x _hd is the real-time position of the master robot, e _{hx is} the deviation between the real-time position of the master robot and the system motion state, The speed of change of the deviation between the real-time position of the master robot and the system motion state;

(2) Aiming at the speed tracking target of the master hand robot, set up the master hand speed tracking controller to track the movement trend of the master hand robot without accurately tracking the position of the human hand:

Among them, u _hv is the input of the master hand speed tracking controller, k _pv is the proportional coefficient of the master hand speed tracking controller, v _hd is the real-time speed of the master hand robot, and e _{hv is} the deviation between the real-time speed of the master hand robot and the system motion speed;

(3) According to the robot's planned trajectory tracking target, set the planned trajectory tracking controller to ensure that the robot-assisted cutting execution process meets the setting of the planned trajectory:

Among them, urx is _the input of the planned trajectory tracking controller, kpr and kdr are the proportional coefficient and differential coefficient of the planned trajectory tracking controller, xrd is _the real-time position of the planned trajectory, _{and erx} is _the deviation between the real-time position of the planned trajectory and the system motion state. It is the speed of change of the deviation between the real-time position of the planned trajectory and the system motion state.

In step S3, a corresponding motion prediction control model is established to obtain the prediction control state of each target controller; including:

in, is the predicted motion state of controller i within the prediction interval, is the predicted motion speed of controller i within the prediction interval, is the predicted acceleration of controller i within the prediction interval, is the predicted control output of controller i within the prediction interval;

When i=1 , the corresponding predictive control state of the master hand position tracking controller is ;

When i=2 , the corresponding predictive control state of the master hand speed tracking controller is ;

When i=3 , the corresponding predicted control state of the robot planning trajectory tracking controller is .

In step S4, according to each predictive control state, a penalty mechanism is introduced to set the evaluation function of each target controller; including:

(1) Establish a target evaluation function for the master hand position tracking controller to evaluate the effectiveness of the master hand position tracking control within the prediction interval:

Among them, J _hx is the cumulative value of the objective function of the master hand position tracking controller in the prediction interval from t ₀ to t ₀ +T , G _hx is the target evaluation function value of the master hand position tracking controller at a single time point in the prediction interval, and x ₀ is the motion state of the system at time t ₀ ; is the predicted control position of the master position tracking control target, which is calculated by rolling forward average according to the current state; P _hx is the penalty function, v ₀ is the system control speed threshold that triggers the penalty, and is used to When , J _hx is reduced, thus limiting the effect of the master hand position tracking controller;

(2) Establish a target evaluation function for the master hand speed tracking controller to evaluate the effectiveness of the master hand speed tracking control within the prediction interval:

Among them, J _hv is the cumulative value of the objective function of the master hand speed tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _{hv is} the target evaluation function value of the master hand speed tracking controller at a single time point in the prediction interval; The predicted control speed of the master hand speed tracking control target is calculated by rolling forward average according to the current state; The predicted control state speed of the master hand speed tracking controller;

(3) Establish a target evaluation function for the robot planning trajectory tracking controller to evaluate the effectiveness of the planned trajectory tracking control within the prediction interval:

Among them, J _rx is the cumulative value of the objective function of the planned trajectory tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _rx is the target evaluation function value of the planned trajectory tracking controller at a single time point in the prediction interval; is the predicted control position of the planned trajectory tracking control target, which is calculated by rolling forward average _of the current state; d0 is the distance threshold for triggering penalty, Prx is the position distance penalty function, which is used When J _rx is reduced, the effect of the planned trajectory tracking controller is limited; Prv is the speed distance penalty function, which is used When _{, Jrx} is reduced, thus limiting the effect of the planned trajectory tracking controller.

In step S5, according to each evaluation function, the predicted evaluation gradient descent value of each control target is calculated accordingly to obtain the authority allocation vector of each target controller in different scenarios; including:

First, the predicted evaluation gradient descent values are calculated for the three control objectives of master hand position tracking, master hand velocity tracking, and robot planning trajectory tracking:

in, is the predicted evaluation gradient descent value of the main hand position tracking control target, is the predicted evaluation gradient descent value of the main hand speed tracking control target, Predict and evaluate the gradient descent value of the robot's trajectory tracking control target;

Then, according to the scene control requirements, it is integrated into the global system input to obtain the permission allocation vector of each target controller in different scenes:

Among them, Ko _is the proportional coefficient matrix, λ is the step size of gradient adjustment, ∇J is the total prediction evaluation gradient descent value, φα is the saturation suppression function, ε is a very small constant, and α _is the threshold constant.

In step S6, the authority allocation vector is used as the input of the state equation to output the fused system motion state and motion speed.

It is not difficult to understand that the permission allocation vector obtained by merging in step S5 Substituting into the state equation formulas (1) and (2), the fused permission allocation vector As the total control input of the aforementioned spring-mass-damper system, the fused system motion state is obtained , the movement speed after fusion Wait for the output results.

In step S7, according to the fused system motion state and the real-time position of the master hand robot, a tactile force acting on the master hand robot is generated based on a force feedback mechanism; including:

in, is the motion state of the fused system, F is the tactile force fed back to the master hand robot, are the error, error change speed and error change acceleration between the fused system motion state and the real-time position of the master hand robot _, respectively. kf , bf _and mf represent the stiffness parameter, damping parameter and inertia parameter of the force feedback system _, respectively.

It can be understood that by introducing the above-mentioned force feedback mechanism, surgeons or other operators can be helped to effectively identify robot-assisted control intentions and dangerous manipulation risk characteristics.

In step S8, the rotation speed of the joint angle of the slave hand robot is generated according to the fused movement speed and the speed of posture change; including:

in, is the fused motion speed, xm represents the fixed distal motion center (i.e., RCM point), Xt represents the _{X -} axis vector of the posture matrix, Yt represents the Y _- axis vector of the posture matrix, Zt represents the Z-axis vector of the posture matrix, qt _represents the posture of the robot, and the function Indicates the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system. is the Euler angle in Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the joint angle of the slave robot (that is, the fusion motion is performed by the slave robot); J ^-1 is the Jacobian matrix, are the joint angles of the slave robot.

In order to better understand the advantages of the method provided by the embodiment of the present invention, the following is an example of a doctor operating a laparoscopic surgical robot to perform a surgical cutting task:

In some cases, robot assistance can enable surgeons to achieve precise cutting operations to meet their requirements for delicate operations or avoid errors, so it is hoped that the trajectory executed by the robot should be closely consistent with the planned path predetermined by the surgeon. On the contrary, in some cases, the surgeon's control is essential to actively adjust the cutting trajectory, such as electrocoagulation hemostasis, active obstacle avoidance, emergency stop, and active suspension of cutting. In these cases, the trajectory executed by the robot should reflect the surgeon's actual operation path.

In this regard, the human-machine safety enhanced shared control method that integrates position, velocity and force, aims at the requirements of fully manual master-slave control scenarios and robot-assisted control scenarios, designs three target controllers including master hand position tracking, master hand velocity tracking and robot planned trajectory tracking, as well as target evaluation functions, and establishes a prediction and evaluation state feedback mechanism based on the current motion state. The gradient descent values of each controller are calculated to generate the robot execution trajectory after motion fusion. Finally, a force feedback mechanism is established based on the deviation between the robot motion trajectory and the actual operation trajectory of the human hand, which helps surgeons effectively identify the robot-assisted control intention and the risk characteristics of dangerous control. The algorithm flow is shown in Figure 2, which takes the liver lobectomy surgery scenario as an example.

The embodiment of the present invention further provides a human-machine safety enhancement shared control device integrating position, speed and force, comprising:

Establish a module to simplify the robot system into a spring-mass-damper system and establish the state equation;

A setting module is used to set three target controllers, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking, according to the target requirements of human-machine shared control;

A prediction module is used to establish a corresponding motion prediction control model to obtain the prediction control state of each target controller;

A setting module is used to introduce a penalty mechanism to set the evaluation function of each target controller according to each predictive control state;

A calculation module is used to calculate the predicted evaluation gradient descent value of each control target according to each evaluation function, so as to obtain the authority allocation vector of each target controller in different scenarios;

An output module is used to use the authority allocation vector as the input of the state equation to output the fused system motion state and motion speed;

A feedback module is used to generate a tactile force acting on the master hand robot based on a force feedback mechanism according to the fused system motion state and the real-time position of the master hand robot;

The generation module is used to generate the rotation speed of the joint angle of the slave hand robot according to the fused movement speed and the speed of posture change.

It can be understood that the human-machine safety enhanced shared control device for integrating position-velocity-force provided in an embodiment of the present invention corresponds to the human-machine safety enhanced shared control method for integrating position-velocity-force provided in an embodiment of the present invention. The explanations, examples and beneficial effects of the relevant contents can refer to the corresponding parts in the human-machine safety enhanced shared control method and will not be repeated here.

In summary, compared with the prior art, the present invention has the following beneficial effects:

1. An embodiment of the present invention provides a method for enhancing human-machine safety shared control by integrating position, speed and force. Three controllers, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking, are introduced to achieve the fusion of "position-speed", thereby improving the human control ability in human-machine shared control. The fusion results of the three controllers are associated with the force feedback mechanism to achieve the fusion of "position-speed-force", thereby enhancing the safety of human-machine interaction in the system.

2. In terms of human hand control, the motion fusion mechanism is used to realize intelligent dynamic switching of autonomous scenes and controllers. The control authority is distributed among the three controllers, and one of the main hand speed control or main hand position control is guaranteed. The operator can also obtain full control at any time by reducing the speed of the main hand robot, thus not depriving the surgeon of control during the operation.

3. In terms of enhancing safety, the design of force feedback enables the shared control mechanism to effectively filter out some hand shaking or non-subjective erroneous operations. In addition, the human hand can obtain the difference between the actual hand position and the motion fusion control output through force feedback, which enhances the force perception ability of the human during robotic surgery and can effectively improve the safety of the surgery.

4. This method shows good capabilities in human-robot shared control tasks, ensuring the controllability, safety, transparency, and adaptability of the surgical robot system. Future work will apply this method to more complex surgical operations and introduce more intelligent controllers to verify the performance of human-robot integration.

It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (9)

1. A method for enhancing human-machine safety shared control by integrating position, speed and force, characterized by comprising: Simplify the robot system into a spring-mass-damper system and establish the state equation; According to the target requirements of human-machine shared control, three target controllers are set, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking; Establish the corresponding motion predictive control model to obtain the predictive control state of each target controller; According to each predictive control state, a penalty mechanism is introduced to set the evaluation function of each target controller; According to each evaluation function, the predicted evaluation gradient descent value of each control target is calculated accordingly to obtain the authority allocation vector of each target controller in different scenarios; wherein the control targets include the master hand position tracking control target, the master hand speed tracking control target and the robot planning trajectory tracking control target; The authority allocation vector is used as the input of the state equation to output the fused system motion state and motion speed; According to the fused system motion state and the real-time position of the master-hand robot, a tactile force acting on the master-hand robot is generated based on a force feedback mechanism; The rotation speed of the joint angle of the slave hand robot is generated according to the fused motion speed and the speed of posture change. 2. The human-machine safety enhanced shared control method according to claim 1, wherein the state equation is: Among them, x is the system motion state, is the system motion speed, is the acceleration of the system, u is the total control input of the system, y is the total control output of the system, m , b and k are the inertia parameter, damping parameter and stiffness parameter of the spring-mass-damper system respectively. 3. The human-machine safety enhanced shared control method according to claim 2, characterized in that, according to the target requirements of human-machine shared control, three target controllers, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking, are set respectively; comprising: Set up the main hand position tracking controller: Among them, u _hx is the input of the master position tracking controller, k _px and k _dx are the proportional coefficient and differential coefficient of the master position tracking controller, x _hd is the real-time position of the master robot, e _{hx is} the deviation between the real-time position of the master robot and the system motion state, The speed of change of the deviation between the real-time position of the master robot and the system motion state; Set up the master hand velocity tracking controller: Among them, u _hv is the input of the master hand speed tracking controller, k _pv is the proportional coefficient of the master hand speed tracking controller, v _hd is the real-time speed of the master hand robot, and e _{hv is} the deviation between the real-time speed of the master hand robot and the system motion speed; Set up the planning trajectory tracking controller: Among them, urx is _the input of the planned trajectory tracking controller, kpr and kdr are the proportional coefficient and differential coefficient of the planned trajectory tracking controller, xrd is _the real-time position of the planned trajectory, _{and erx} is _the deviation between the real-time position of the planned trajectory and the system motion state. It is the speed of change of the deviation between the real-time position of the planned trajectory and the system motion state. 4. The human-machine safety enhanced shared control method according to claim 3, characterized in that the establishment of a corresponding motion prediction control model to obtain the prediction control state of each target controller comprises: in, is the predicted motion state of controller i within the prediction interval, is the predicted motion speed of controller i within the prediction interval, is the predicted acceleration of controller i within the prediction interval, is the predicted control output of controller i within the prediction interval; When i=1 , the corresponding predictive control state of the master hand position tracking controller is ; When i=2 , the corresponding predictive control state of the master hand speed tracking controller is ; When i=3 , the corresponding predicted control state of the robot planning trajectory tracking controller is . 5. The human-machine safety enhanced shared control method according to claim 4, characterized in that the penalty mechanism is introduced to set the evaluation function of each target controller according to each predicted control state; comprising: Establish the target evaluation function for the master hand position tracking controller: Among them, J _hx is the cumulative value of the objective function of the master hand position tracking controller in the prediction interval from t ₀ to t ₀ +T , G _hx is the target evaluation function value of the master hand position tracking controller at a single time point in the prediction interval, and x ₀ is the motion state of the system at time t ₀ ; is the predicted control position of the master position tracking control target, which is calculated by rolling forward average according to the current state; P _hx is the penalty function, v ₀ is the system control speed threshold that triggers the penalty, and is used to When J _hx is reduced; Establish the target evaluation function for the master hand speed tracking controller: Among them, J _hv is the cumulative value of the objective function of the master hand speed tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _{hv is} the target evaluation function value of the master hand speed tracking controller at a single time point in the prediction interval; The predicted control speed of the master hand speed tracking control target is calculated by rolling forward average according to the current state; The predicted control state speed of the master hand speed tracking controller; Establish the target evaluation function for the robot planning trajectory tracking controller: Among them, J _rx is the cumulative value of the objective function of the planned trajectory tracking controller in the prediction interval from t ₀ to t ₀ +T , and G _rx is the target evaluation function value of the planned trajectory tracking controller at a single time point in the prediction interval; is the predicted control position of the planned trajectory tracking control target, which is calculated by rolling forward average _of the current state; d0 is the distance threshold for triggering penalty, Prx is the position distance penalty function, which is used When J _rx is reduced; Prv is the speed distance penalty function, used When J _rx is reduced. 6. The human-machine safety enhanced shared control method according to claim 5, characterized in that the predicted evaluation gradient descent value of each control target is calculated according to each evaluation function to obtain the authority allocation vector of each target controller in different scenarios; comprising: The predicted evaluation gradient descent values are calculated for the three control objectives of master hand position tracking, master hand velocity tracking, and robot planning trajectory tracking: in, is the predicted evaluation gradient descent value of the main hand position tracking control target, is the predicted evaluation gradient descent value of the main hand speed tracking control target, Predict and evaluate the gradient descent value of the robot's trajectory tracking control target; According to the scene control requirements, it is integrated into the global system input to obtain the permission allocation vector of each target controller in different scenes: Among them, Ko _is the proportional coefficient matrix, λ is the step size of gradient adjustment, ∇J is the total prediction evaluation gradient descent value, φα is the saturation suppression function, ε is a very small constant, and α _is the threshold constant. 7. The human-machine safety enhanced shared control method according to claim 6, characterized in that the tactile force acting on the master hand robot is generated based on a force feedback mechanism according to the fused system motion state and the real-time position of the master hand robot; comprising: in, is the motion state of the fused system, F is the tactile force fed back to the master hand robot, are the error, error change speed and error change acceleration between the fused system motion state and the real-time position of the master hand robot _, respectively. kf , bf _and mf represent the stiffness parameter, damping parameter and inertia parameter of the force feedback system _, respectively. 8. The human-machine safety enhanced shared control method according to claim 6, characterized in that the rotation speed of the joint angle of the slave hand robot is generated according to the fused motion speed and the speed of posture change; comprising: in, is the fused motion speed, xm represents the fixed distal motion center, Xt represents the _{X -} axis vector of the posture matrix, Yt represents the Y _- axis vector of the posture matrix, Zt represents the Z-axis vector of the posture matrix, qt _represents the posture of the robot, and the function Indicates the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system. is the Euler angle in Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the joint angle of the slave robot; J ^-1 is the Jacobian matrix, are the joint angles of the slave robot. 9. A human-machine safety enhancement shared control device integrating position, speed and force, characterized by comprising: Establish a module to simplify the robot system into a spring-mass-damper system and establish the state equation; A setting module is used to set three target controllers, namely, master hand position tracking, master hand speed tracking and robot planning trajectory tracking, according to the target requirements of human-machine shared control; A prediction module is used to establish a corresponding motion prediction control model to obtain the prediction control state of each target controller; A setting module is used to introduce a penalty mechanism to set the evaluation function of each target controller according to each predictive control state; A calculation module is used to calculate the predicted evaluation gradient descent value of each control target according to each evaluation function, so as to obtain the authority allocation vector of each target controller in different scenarios; wherein the control targets include the master hand position tracking control target, the master hand speed tracking control target and the robot planning trajectory tracking control target; An output module is used to use the authority allocation vector as the input of the state equation to output the fused system motion state and motion speed; A feedback module is used to generate a tactile force acting on the master hand robot based on a force feedback mechanism according to the fused system motion state and the real-time position of the master hand robot; The generation module is used to generate the rotation speed of the joint angle of the slave hand robot according to the fused movement speed and the speed of posture change.