Compliant control method and system for master-slave robot, electronic device and storage medium

Abstract

The application discloses a kind of master-slave robot compliant control method, system, electronic equipment and storage medium, the compliant control method includes: obtaining the environment force of master robot;The environment force is input to the joint state prediction model of the master robot to obtain the master joint position increment of the master robot;The joint state prediction model is obtained based on impedance control model and robot dynamics model construction;The master joint position increment is mapped to the joint of slave robot as slave joint position increment to control the slave robot.The compliant control method of master-slave robot of the application, using impedance control strategy based on external interaction force, by collecting the speed and force of master robot, predict the position and posture of next step master robot, make master robot meet medical application scene requirement, send the pose information to slave robot, realize the compliant control function of master-slave robot, while realizing the following control of slave robot.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a compliant control method, system, electronic device, and storage medium for a master-slave robot. Background Technology

[0002] With the development of medical technology and the growing demand for medical services in remote areas, there is an urgent need for surgical robots capable of master-slave and remote operation. To meet the safety requirements of human-computer interaction, medical robots generally choose collaborative robots as the actuators to assist doctors in surgical procedures. A typical master-slave surgical robot system mainly consists of an operator, a master robot system, a network information transmission channel, a slave robot system, and the external working environment.

[0003] In practical applications, the robot's position is usually acquired in real time and mapped to the slave robot through the end position or joint position and posture. However, factors such as communication rate and acquisition cycle can cause delays in position and posture or loss of process posture, which seriously affects the follow-up effect and accuracy of the surgical robot. Therefore, the control method needs to be designed not only to enable the robot to complete the work objectives, but also to avoid jitter and vibration problems during command transmission, so as to ensure the stability of the system. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to overcome the defects of the master-slave robot system in the prior art, which will cause delay in position and posture or loss of process posture due to factors such as communication rate and acquisition cycle. The invention provides a compliant control method, system, electronic device and storage medium for master-slave robots.

[0005] The present invention solves the above-mentioned technical problems through the following technical solution:

[0006] This invention provides a compliant control method for a master-slave robot, the compliant control method comprising:

[0007] Obtain the environmental forces of the master robot;

[0008] The environmental forces are input into the joint state prediction model of the master robot to obtain the position increment of the main joint of the master robot; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model;

[0009] The master joint position increment is mapped as a slave joint position increment to the joints of the slave robot to control the slave robot.

[0010] Preferably, the steps for acquiring the environmental forces of the master robot include:

[0011] The environmental forces and torques at the end of the master robot are obtained using a six-dimensional force sensor; the six-dimensional force sensor is installed at the end of the master robot.

[0012] Preferably, prior to the step of acquiring the environmental forces and torques at the end effector of the master robot using a six-dimensional force sensor, the compliant control method includes:

[0013] The initial end-effector load, spatial pose, multiple sets of measured forces and measured torques of the master robot after the six-dimensional force sensor is set are obtained to calculate the initial force and initial torque of the six-dimensional force sensor.

[0014] The step of acquiring the environmental forces and environmental torques at the end of the master robot using a six-dimensional force sensor includes:

[0015] The current environmental force and current environmental torque are determined based on the currently measured force and torque, and the initial force and initial torque.

[0016] Preferably, before the step of inputting the environmental force into the joint state prediction model of the master robot, the compliant control method further includes:

[0017] The joint dynamics model is obtained based on the impedance control model and the inverse Jacobian matrix;

[0018] The robot dynamics model is constructed based on the joint dynamics model;

[0019] The robot dynamics model is subjected to model predictive control based on joint boundary constraints to obtain the joint state prediction model.

[0020] Preferably, the joint dynamics model is obtained through the following formula:

[0021] e f =f d -f s ;

[0022]

[0023]

[0024] Among them, f d For the desired force, f s e represents the environmental force obtained by the six-dimensional force sensor. f Error of force change rate of master robot, k s To control the stiffness coefficient, b s J is the damping coefficient. -1 It is the inverse Jacobian matrix; and / or,

[0025] The robot dynamics model is obtained using the following formula:

[0026] q d =q1+q;

[0027]

[0028] Where q is the angular displacement. Angular velocity, τ is angular acceleration; d The force required to control the robot's movement, H(q) d ) represents the coefficients of the inertia matrix, C(q) d ,q d G(q) represents the coefficients of the Copernicus matrix. d ) are the gravity matrix coefficients; and / or,

[0029] The joint state prediction model is obtained using the following formula:

[0030]

[0031]

[0032]

[0033]

[0034] Among them, J k ,K k M k For the matrix coefficients of the linearization transformation, A k B k c represents the coefficients of the state iteration equation; and / or,

[0035] The joint state prediction model is matrixed using the following formula:

[0036] Q d =AQ k +Bf+C;

[0037] in,

[0038] τ=[τ k τ k+1 τ k+2 …τ k+h ] T ;

[0039] A = [AA] 2 A 3 … A h ] T ;

[0040]

[0041]

[0042] Preferably, after the step of inputting the environmental forces into the joint state prediction model of the master robot, the compliant control method further includes:

[0043] Construct the cost function of the joint state prediction model and add constraints;

[0044] Solve the cost function to obtain the position increment of the main joint.

[0045] Preferably, the cost function and constraints are constructed using the following formula:

[0046]

[0047] stP*Q d ≤b;

[0048]

[0049] The present invention also provides a compliant control system for a master-slave robot, characterized in that the compliant control system includes:

[0050] The environmental force acquisition module is used to acquire the environmental forces of the master robot.

[0051] The position increment determination module is used to input the environmental force into the joint state prediction model of the master robot to obtain the position increment of the main joint of the master robot; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model;

[0052] The position increment mapping module is used to map the master joint position increment as a slave joint position increment to the joints of the slave robot in order to control the slave robot.

[0053] Preferably, the environmental force acquisition module is specifically used to acquire the environmental force and environmental torque at the end of the master robot using a six-dimensional force sensor; the six-dimensional force sensor is disposed at the end of the master robot.

[0054] Preferably, the compliance control system includes:

[0055] The initial value calculation module is used to obtain the initial end load, spatial pose, multiple sets of measured forces and measured torques of the master robot after setting the six-dimensional force sensor in order to calculate the initial force and initial torque of the six-dimensional force sensor.

[0056] The environmental force acquisition module is specifically used to determine the current environmental force and current environmental torque based on the currently measured force and torque, and the initial force and initial torque.

[0057] Preferably, the compliant control system further includes a predictive model building module;

[0058] The prediction model building module is used to obtain a joint dynamics model based on the impedance control model and the inverse Jacobian matrix;

[0059] The prediction model building module is also used to build the robot dynamics model based on the joint dynamics model;

[0060] The prediction model building module is also used to perform model predictive control on the robot dynamics model based on joint boundary constraints to obtain the joint state prediction model.

[0061] Preferably, the joint dynamics model is obtained through the following formula:

[0062] e f =f d -f s ;

[0063]

[0064]

[0065] Among them, f d For the desired force, f s e represents the environmental force obtained by the six-dimensional force sensor. f Error of force change rate of master robot, k s To control the stiffness coefficient, b s J is the damping coefficient. -1 It is the inverse Jacobian matrix; and / or,

[0066] The robot dynamics model is obtained using the following formula:

[0067] q d =q1+q;

[0068]

[0069] Where q is the angular displacement. Angular velocity, τ is angular acceleration; d The force required to control the robot's movement, H(q) d ) represents the coefficients of the inertia matrix, C(q) d ,q d G(q) represents the coefficients of the Copernicus matrix. d) are the gravity matrix coefficients; and / or,

[0070] The joint state prediction model is obtained using the following formula:

[0071]

[0072]

[0073]

[0074]

[0075] Among them, J k ,K k M k For the matrix coefficients of the linearization transformation, A k B k c represents the coefficients of the state iteration equation; and / or,

[0076] The joint state prediction model is matrixed using the following formula:

[0077] Q d =AQ k +Bf+C;

[0078] in,

[0079] τ=[τ k τ k+1 τ k+2 …τ k+h ] T ;

[0080] A = [AA] 2 A 3 … A h ] T ;

[0081]

[0082]

[0083] Preferably, the compliant control system further includes:

[0084] The cost function construction module is used to construct the cost function of the joint state prediction model and add constraints.

[0085] The cost function analysis module is used to solve the cost function to obtain the position increment of the main joint.

[0086] Preferably, the cost function and constraints are constructed using the following formula:

[0087]

[0088] stP*Q d ≤b;

[0089]

[0090] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the compliant control method for a master-slave robot as described above.

[0091] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the compliant control method for a master-slave robot as described above.

[0092] The positive and progressive effects of this invention are as follows:

[0093] The compliant control method for master-slave robots provided by this invention adopts an impedance control strategy based on external interactive forces. By collecting the speed and force of the master robot, the next position and posture of the master robot are predicted, so that the master robot meets the requirements of medical application scenarios. The posture information is then sent to the slave robot, thereby realizing the compliant control function of the master-slave robot and the following control of the slave robot. Attached Figure Description

[0094] To more clearly illustrate the technical solutions of the embodiments in this specification, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are merely some examples or embodiments of this specification. For those skilled in the art, these drawings can be applied to other similar scenarios without creative effort.

[0095] Figure 1 This is the first flowchart of the compliant control method for master-slave robots in Embodiment 1 of the present invention.

[0096] Figure 2 This is a schematic diagram of the compliant control method for master-slave robots in Embodiment 1 of the present invention.

[0097] Figure 3 This is the second flowchart of the compliant control method for the master-slave robot in Embodiment 1 of the present invention.

[0098] Figure 4 This is a first structural schematic diagram of the compliant control system of the master-slave robot in Embodiment 2 of the present invention.

[0099] Figure 5This is a first structural schematic diagram of the compliant control system of the master-slave robot in Embodiment 2 of the present invention.

[0100] Figure 6 This is a schematic diagram of the electronic device in Embodiment 3 of the present invention. Detailed Implementation

[0101] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the embodiments described herein.

[0102] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the document does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0103] It should be understood that the terms “system,” “device,” “unit,” and / or “module” used herein are one way to distinguish different components, elements, parts, sections, or assemblies at different levels. However, if other terms can achieve the same purpose, they may be replaced by other expressions.

[0104] As illustrated herein, unless the context clearly indicates otherwise, the words “a,” “an,” “an,” and / or “the” do not specifically refer to the singular and may also include the plural. Generally speaking, the terms “comprising” and “including” only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0105] The definitions used herein, such as the terms “having,” “may have,” “comprising,” or “may include,” indicate the presence of the corresponding function, operation, element, etc., and do not limit the presence of one or more other functions, operations, elements, etc. Furthermore, it should be understood that the terms “comprising” or “having” as used herein indicate the presence of the features, figures, steps, operations, elements, components, or combinations thereof described in the specification, without excluding the presence or addition of one or more other features, figures, steps, operations, elements, components, or combinations thereof.

[0106] The definitions of "A or B", "at least one of A and / or B" or "one or more of A and / or B" used herein include any and all combinations of the words listed therewith. For example, "A or B", "at least one of A and B" or "at least one of A or B" means (1) including at least one A, (2) including at least one B, or (3) including at least one A and at least one B.

[0107] Flowcharts are used in this document to illustrate the operations performed by the system according to the embodiments herein. It should be understood that the preceding or following operations are not necessarily performed in exact order. Instead, the steps can be processed in reverse order or simultaneously. Furthermore, other operations can be added to these processes, or one or more steps can be removed from them.

[0108] Example 1

[0109] Please refer to Figure 1 This is the first flowchart of the compliant control method for the master-slave robot in this embodiment. Specifically, as shown... Figure 1 As shown, the compliance control method includes:

[0110] S101, Obtain the environmental forces of the master robot.

[0111] S102. Input environmental forces into the joint state prediction model of the master robot to obtain the position increment of the main joint of the master robot; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model.

[0112] S103. Map the master joint position increment as the slave joint position increment to the joint of the slave robot to control the slave robot.

[0113] Please refer to Figure 2 This is a schematic diagram of the compliant control method for the master-slave robot in this embodiment. Specifically, as shown... Figure 2 As shown, a robot predictive controller is constructed based on an impedance controller and a robot dynamics model. By collecting the speed of the master robot and the environmental forces, the controller predicts the position and posture of the master robot in the next step, realizing the adaptive control of the master robot to the environment. The robot also sends the posture information to the slave robot, realizing the compliant control function of the master and slave robots, while also realizing the following control of the slave robot.

[0114] In one optional implementation, step S101 includes:

[0115] A six-dimensional force sensor is used to acquire the environmental forces and torques at the end effector of the master robot. The six-dimensional force sensor is installed at the end effector of the master robot. Specifically, the six-dimensional force sensor, also called a six-dimensional force and torque sensor, is a device that can simultaneously measure force and torque in a Cartesian coordinate system and convert each of its three components into an electrical signal.

[0116] Please refer to Figure 3 This is the second flowchart of the compliant control method for the master-slave robot in this embodiment. Specifically, as shown... Figure 3 As shown, in another optional embodiment, prior to step S101, the compliance control method includes:

[0117] S001. Obtain the initial end effector load, spatial pose, and multiple sets of measured forces and torques of the master-end robot after setting up the six-dimensional force sensor to calculate the initial force and initial torque of the six-dimensional force sensor. Specifically, the preprocessing steps for the force sensor at the robot's end effector are as follows:

[0118] A. The weight of the robot's end-effector load (the load installed after the six-dimensional force sensor at the end) was measured as Mg by weighing.

[0119] B. Obtain the sensor's pose in the robot's base coordinate system through kinematic relationships:

[0120] Psensor(Psensor.x,Psensor.y,Psensor.z,Psensor.rx,Psensor.ry,Psensor.rz);

[0121] C. Calculate the components of the end load gravity in the X, Y, and Z directions of the sensor coordinate system: Mgx=(-cos(Psensor.rx)×sin(Psensor.ry)×cos(Psensor.rz)+sin(Psensor.rx)×sin(Psensor.rz))×(-Mg)Mgy=(cos(Psensor.rx)×sin(Psensor.ry)×sin(Psensor.rz)+sin(Psensor.rx)×cos(Psensor.rz))×(-Mg)Mgz=(cos(Psensor.rx)×cos(Psensor.ry))×(-Mg).

[0122] D. The force sensor (Sensor.F) and torque sensor (Sensor.T) measured by the six-dimensional force sensor are:

[0123] (Sensor.Fx,Sensor.Fy,Sensor.Fz,Sensor.Tx,Sensor.Ty,Sensor.Tz).

[0124] E. Calculate the force initialization value InF of the six-dimensional force sensor:

[0125] InFx = Sensor.Fx - Mgx

[0126] InFy = Sensor.Fy - Mgy

[0127] InFz = Sensor.Fz - Mgz

[0128] Because the gravitational torque generated by the end load has little impact on the initial calibration of the sensor torque, the initial torque value InT of the six-dimensional force sensor is directly taken from the measured torque value Sensor.T.

[0129] InTx = Sensor.Tx

[0130] InTy = Sensor.Ty

[0131] InTz = Sensor.Tz

[0132] Finally, several sets of average values ​​are used to improve the accuracy of the force / torque initialization values ​​of the six-dimensional force sensor.

[0133] At this time, step S101 includes:

[0134] S1011. Determine the current environmental force and current environmental moment based on the currently measured force and torque, the initial force and initial torque. Specifically, determine the environmental force ExF according to the following formula.

[0135] ExFx = Sensor.Fx - InFx

[0136] ExFy = Sensor.Fy - InFy

[0137] ExFz = Sensor.Fz - InFz

[0138] In an optional implementation, prior to step S102, the compliance control method further includes:

[0139] S002. The joint dynamics model is obtained based on the impedance control model and the inverse Jacobian matrix;

[0140] S003. Constructing a robot dynamics model based on a joint dynamics model;

[0141] S004. Based on the joint boundary constraints, perform model predictive control on the robot dynamics model to obtain the joint state prediction model.

[0142] Specifically, the joint dynamics model is obtained through the following formula:

[0143] e f =f d -f s ;

[0144]

[0145]

[0146] Among them, f d For the desired force, f s For the environmental forces obtained by the six-dimensional force sensor, e f Error of force change rate of master robot, k s To control the stiffness coefficient, b s J is the damping coefficient. -1 It is the inverse Jacobian matrix;

[0147] The robot's dynamic model is obtained using the following formula:

[0148] q d =q1+q;

[0149]

[0150] Where q is the angular displacement. Angular velocity, τ is angular acceleration; d The force required to control the robot's movement, H(q) d ) represents the coefficients of the inertia matrix, C(q) d ,q d G(q) represents the coefficients of the Copernicus matrix. d ) represents the coefficients of the gravity matrix;

[0151] Since each joint has boundary constraints, the MPC (Model Predictive Control) predictive control method is used to predict the next joint change Δq. d1 The specific steps are as follows, where angular velocity and angular velocity can be respectively:

[0152]

[0153]

[0154] The above impedance control dynamics model can be transformed into:

[0155]

[0156] Simplifying, we obtain a matrix form, which serves as the state iteration equation for predictive control:

[0157]

[0158] Among them, J k ,K k M k For the matrix coefficients of the linearization transformation, A k B k c represents the coefficients of the state iteration equation;

[0159] The joint state prediction model is matrixed using the following formula:

[0160] Q d =AQ k +Bf+C;

[0161] in,

[0162] τ=[τ k τ k+1 τ k+2 …τ k+h ] T ;

[0163] A = [AA] 2 A 3 … A h ] T ;

[0164]

[0165]

[0166] In an optional implementation, after step S102, the compliance control method further includes:

[0167] S005. Construct the cost function of the joint state prediction model and add constraints.

[0168] S006. Solve the cost function to obtain the main joint position increment.

[0169] Specifically, the cost function is constructed using the following formula:

[0170]

[0171] The constraints are:

[0172] stP*Q d ≤b;

[0173] Among them, Q ref Let the reference angular position be a constant coefficient vector, L and S be the coefficient matrices of the cost function, P be the angular constraint coefficient matrix, and Q be... d Let angular position vector be the vector, and the solution be... get Will get As the position increment input from the slave joint, it is fed into the slave robot joint to achieve the effect of slave-end following.

[0174] The compliant control method for master-slave robots provided in this embodiment adopts an impedance control strategy based on external interactive forces. By collecting the speed and force of the master robot, the next position and posture of the master robot are predicted, so that the master robot meets the requirements of the medical application scenario. The posture information is sent to the slave robot, realizing the compliant control function of the master and slave robots while realizing the follow control of the slave robot. The sensor is calibrated based on the data compensation method of the six-dimensional force sensor to accurately calculate the force and torque data in each direction.

[0175] Example 2

[0176] Please refer to Figure 4 This is a first structural schematic diagram of the compliant control system of the master-slave robot in this embodiment. Specifically, as shown... Figure 4 As shown, the compliance control system includes:

[0177] Environmental force acquisition module 1 is used to acquire the environmental forces of the master robot.

[0178] Position increment determination module 2 is used to input environmental forces into the joint state prediction model of the master robot to obtain the position increment of the master robot's main joints; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model;

[0179] Position increment mapping module 3 is used to map the position increment of the master joint as the position increment of the slave joint to the joint of the slave robot in order to control the slave robot.

[0180] Please refer to Figure 2 This is a schematic diagram of the compliant control system of the master-slave robot in this embodiment. Specifically, as shown... Figure 2 As shown, a robot predictive controller is constructed based on an impedance controller and a robot dynamics model. By collecting the speed of the master robot and the environmental forces, the controller predicts the position and posture of the master robot in the next step, realizing the adaptive control of the master robot to the environment. The robot also sends the posture information to the slave robot, realizing the compliant control function of the master and slave robots, while also realizing the following control of the slave robot.

[0181] In one optional implementation, the environmental force acquisition module 1 is specifically used to acquire the environmental force and environmental torque at the end of the master robot using a six-dimensional force sensor; the six-dimensional force sensor is located at the end of the master robot. Specifically, the six-dimensional force sensor, also called a six-dimensional force and torque sensor, is a device that can simultaneously measure force and torque in a Cartesian coordinate system and convert each of its three components into an electrical signal.

[0182] Please refer to Figure 5 This is a schematic diagram of the second structure of the compliant control system of the master-slave robot in this embodiment. Specifically, as shown... Figure 5 As shown, in another alternative embodiment, the compliance control system includes:

[0183] The initial value calculation module 4 is used to obtain the initial end load, spatial pose, multiple sets of measured forces and measured torques of the master robot after setting the six-dimensional force sensor in order to calculate the initial force and initial torque of the six-dimensional force sensor.

[0184] Specifically, the preprocessing steps for the force sensor at the robot's end effector are as follows:

[0185] A. The weight of the robot's end-effector load (the load installed after the six-dimensional force sensor at the end) was measured as Mg by weighing.

[0186] B. Obtain the sensor's pose in the robot's base coordinate system through kinematic relationships:

[0187] Psensor(Psensor.x,Psensor.y,Psensor.z,Psensor.rx,Psensor.ry,Psensor.rz);

[0188] C. Calculate the components of the end load gravity in the X, Y, and Z directions of the sensor coordinate system: Mgx=(-cos(Psensor.rx)×sin(Psensor.ry)×cos(Psensor.rz)+sin(Psensor.rx)×sin(Psensor.rz))×(-Mg)Mgy=(cos(Psensor.rx)×sin(Psensor.ry)×sin(Psensor.rz)+sin(Psensor.rx)×cos(Psensor.rz))×(-Mg)Mgz=(cos(Psensor.rx)×cos(Psensor.ry))×(-Mg).

[0189] D. The force / torque measured by the six-dimensional force sensor is:

[0190] (Sensor.Fx,Sensor.Fy,Sensor.Fz,Sensor.Tx,Sensor.Ty,Sensor.Tz).

[0191] E. Calculate the initial force / torque values ​​for the six-dimensional force sensor:

[0192] InFx = Sensor.Fx - Mgx

[0193] InFy = Sensor.Fy - Mgy

[0194] InFz = Sensor.Fz - Mgz

[0195] Because the gravitational torque generated by the end load has little impact on the initial calibration of the sensor torque, the initial torque value of the six-dimensional force sensor is directly taken from the measured torque value:

[0196] InTx = Sensor.Tx

[0197] InTy = Sensor.Ty

[0198] InTz = Sensor.Tz

[0199] Finally, several sets of average values ​​are used to improve the accuracy of the force / torque initialization values ​​of the six-dimensional force sensor.

[0200] At this point, the environmental force acquisition module 1 is specifically used to determine the current environmental force and current environmental moment based on the currently measured force and torque, the initial force and initial torque. Specifically, the environmental force is determined according to the following formula.

[0201] ExFx = Sensor.Fx - InFx

[0202] ExFy = Sensor.Fy - InFy

[0203] ExFz = Sensor.Fz - InFz

[0204] In one alternative implementation, the compliant control system further includes a predictive model building module 5;

[0205] Prediction model building module 5 is used to obtain the joint dynamics model based on the impedance control model and the inverse Jacobian matrix;

[0206] Prediction model building module 5 is also used to build robot dynamics models based on joint dynamics models;

[0207] The prediction model building module 5 is also used to perform model predictive control on the robot dynamics model based on joint boundary constraints to obtain a joint state prediction model.

[0208] Specifically, the joint dynamics model is obtained through the following formula:

[0209] e f =f d -f s ;

[0210]

[0211]

[0212] Among them, f d For the desired force, f s For the environmental forces obtained by the six-dimensional force sensor, e f Error of force change rate of master robot, k s To control the stiffness coefficient, b s J is the damping coefficient. -1 It is the inverse Jacobian matrix;

[0213] The robot's dynamic model is obtained using the following formula:

[0214] q d =q1+q;

[0215]

[0216] Where q is the angular displacement. Angular velocity, τ is angular acceleration; d The force required to control the robot's movement, H(q) d ) represents the coefficients of the inertia matrix, C(q) d ,q d G(q) represents the coefficients of the Copernicus matrix. d ) represents the coefficients of the gravity matrix;

[0217] Since each joint has boundary constraints, the MPC (Model Predictive Control) predictive control method is used to predict the next joint change Δq. d1 Specifically, angular velocity and angular velocity can be respectively:

[0218]

[0219]

[0220] The above impedance control dynamics model can be transformed into:

[0221]

[0222] Simplifying, we obtain a matrix form, which serves as the state iteration equation for predictive control:

[0223]

[0224] Among them, J k ,K k M k For the matrix coefficients of the linearization transformation, A k B kc represents the coefficients of the state iteration equation;

[0225] The joint state prediction model is matrixed using the following formula:

[0226] Q d =AQ k +Bf+C;

[0227] in,

[0228] τ=[τ k τ k+1 τ k+2 …τ k+h ] T ;

[0229] A = [AA] 2 A 3 … A h ] T ;

[0230]

[0231]

[0232] In one alternative implementation, the compliance control system further includes:

[0233] Cost function construction module 6 is used to construct the cost function of the joint state prediction model and add constraints.

[0234] Cost function analysis module 7 is used to solve the cost function to obtain the main joint position increment.

[0235] Preferably, the cost function is constructed using the following formula:

[0236]

[0237] The constraints are:

[0238] stP*Q d ≤b;

[0239] Among them, Q ref Let the reference angular position be a constant coefficient vector, L and S be the coefficient matrices of the cost function, P be the angular constraint coefficient matrix, and Q be... d Let angular position vector be the vector, and the solution be... get Will get As the position increment input from the slave joint, it is fed into the slave robot joint to achieve the effect of slave-end following.

[0240] The compliant control system for the master-slave robot provided in this embodiment adopts an impedance control strategy based on external interactive forces. By collecting the speed and force of the master robot, the system predicts the next position and posture of the master robot, ensuring that the master robot meets the requirements of the medical application scenario. The posture information is then sent to the slave robot, realizing the compliant control function of the master-slave robot while enabling the slave robot to follow. The system also uses a data compensation method based on a six-dimensional force sensor to calibrate the sensor and accurately calculate the force and torque data in each direction.

[0241] Example 3

[0242] Figure 6 This is a schematic diagram of an electronic device provided in Embodiment 3 of the present invention. The electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the compliant control method of the master-slave robot in Embodiment 1. Figure 6 The electronic device 30 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of the present invention.

[0243] like Figure 6 As shown, the electronic device 30 can be manifested as a general-purpose computing device, such as a server device. The components of the electronic device 30 may include, but are not limited to: at least one processor 31, at least one memory 32, and a bus 33 connecting different system components (including memory 32 and processor 31).

[0244] Bus 33 includes a data bus, an address bus, and a control bus.

[0245] The memory 32 may include volatile memory, such as random access memory (RAM) 321 and / or cache memory 322, and may further include read-only memory (ROM) 323.

[0246] The memory 32 may also include a program / utility 325 having a set (at least one) of program modules 324, including but not limited to: an operating system, one or more application programs, other program modules, and program data, each or some combination of these examples may include an implementation of a network environment.

[0247] The processor 31 executes various functional applications and data processing by running computer programs stored in the memory 32, such as the compliant control method of the master-slave robot in Embodiment 1 of the present invention.

[0248] Electronic device 30 can also communicate with one or more external devices 34 (e.g., keyboard, pointing device, etc.). This communication can be performed via input / output (I / O) interface 35. Furthermore, the model-generated device 30 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public network, such as the Internet) via network adapter 36. As shown, network adapter 36 communicates with other modules of the model-generated device 30 via bus 33. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in conjunction with the model-generated device 30, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems.

[0249] It should be noted that although several units / modules or sub-units / modules of the electronic device have been mentioned in the detailed description above, this division is merely exemplary and not mandatory. In fact, according to embodiments of the present invention, the features and functions of two or more units / modules described above can be embodied in one unit / module. Conversely, the features and functions of one unit / module described above can be further divided and embodied by multiple units / modules.

[0250] Example 4

[0251] This embodiment provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the compliant control method of the master-slave robot of Embodiment 1.

[0252] The readable storage medium may be more specifically adopted, including but not limited to: portable disk, hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0253] In a possible implementation, the present invention can also be implemented as a program product comprising program code, which, when the program product is run on a terminal device, is used to cause the terminal device to execute the compliant control method for the master-slave robot of Embodiment 1.

[0254] The program code for executing the present invention can be written in any combination of one or more programming languages. The program code can be executed entirely on the user device, partially on the user device, as a standalone software package, partially on the user device and partially on a remote device, or entirely on a remote device.

[0255] While specific embodiments of the present invention have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of the present invention is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of the present invention, but all such changes and modifications fall within the scope of protection of the present invention.

Claims

1. A compliant control method for a master-slave robot, characterized in that, The compliance control method includes: Obtain the environmental forces of the master robot; The environmental forces are input into the joint state prediction model of the master robot to obtain the position increment of the main joint of the master robot; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model; The master joint position increment is mapped as a slave joint position increment to the joint of the slave robot to control the slave robot; Before the step of inputting the environmental force into the joint state prediction model of the master robot, the compliant control method further includes: The joint dynamics model is obtained based on the impedance control model and the inverse Jacobian matrix; The robot dynamics model is constructed based on the joint dynamics model; Based on the joint boundary constraints, the robot dynamics model is subjected to model predictive control to obtain the joint state prediction model. The joint dynamics model is obtained through the following formula: ； ； ； in, For the desired force, For the environmental force, Force error of the master robot, To control the stiffness coefficient, The damping coefficient is... It is the inverse Jacobian matrix; The robot dynamics model is obtained using the following formula: ； ； in, It is an angular displacement. Angular velocity, Angular acceleration; The force required to control the robot's movement, These are the coefficients of the inertia matrix. These are the coefficients of the gravity matrix; The joint state prediction model is obtained using the following formula: ； ； ； ； in, , , The coefficients of the linearization transformation matrix. , , These are the coefficients of the state iteration equation; The joint state prediction model is matrixed using the following formula: ； in, ; ； ； ； ； After the step of inputting the environmental forces into the joint state prediction model of the master robot, the compliant control method further includes: Construct the cost function of the joint state prediction model and add constraints; Solve the cost function to obtain the main joint position increment; The cost function and constraints are constructed using the following formula: ； ； ； in, The reference angular position is a vector with constant coefficients. S Let P be the coefficient matrix of the cost function, and let P be the angular constraint coefficient matrix. is the angular position vector.

2. The compliance control method as described in claim 1, characterized in that, The steps to obtain the environmental forces of the master robot include: The environmental forces and torques at the end of the master robot are obtained using a six-dimensional force sensor; the six-dimensional force sensor is installed at the end of the master robot.

3. The compliance control method as described in claim 2, characterized in that, Prior to the step of acquiring the environmental forces and torques at the end effector of the master robot using a six-dimensional force sensor, the compliant control method includes: The initial end-effector load, spatial pose, multiple sets of measured forces and measured torques of the master robot after the six-dimensional force sensor is set are obtained to calculate the initial force and initial torque of the six-dimensional force sensor. The step of acquiring the environmental forces and environmental torques at the end of the master robot using a six-dimensional force sensor includes: The current environmental force and current environmental torque are determined based on the currently measured force and torque, and the initial force and initial torque.

4. A compliant control system for a master-slave robot, characterized in that, The compliance control system includes: The environmental force acquisition module is used to acquire the environmental forces of the master robot. The position increment determination module is used to input the environmental force into the joint state prediction model of the master robot to obtain the position increment of the main joint of the master robot; the joint state prediction model is constructed based on the impedance control model and the robot dynamics model; A position increment mapping module is used to map the master joint position increment as a slave joint position increment to the joints of the slave robot in order to control the slave robot. The compliant control system also includes a predictive model building module; The prediction model building module is used to obtain a joint dynamics model based on the impedance control model and the inverse Jacobian matrix; The prediction model building module is also used to build the robot dynamics model based on the joint dynamics model; The prediction model building module is also used to perform model prediction control on the robot dynamics model based on joint boundary constraints to obtain the joint state prediction model. The joint dynamics model is obtained through the following formula: ； ； ； in, For the desired force, For the environmental force, Force error of the master robot, To control the stiffness coefficient, The damping coefficient is... It is the inverse Jacobian matrix; The robot dynamics model is obtained using the following formula: ； ； in, It is an angular displacement. Angular velocity, Angular acceleration; The force required to control the robot's movement, For the coefficients of the inertia matrix, These are the coefficients of the gravity matrix; The joint state prediction model is obtained using the following formula: ； ； ； ； in, , , The coefficients of the linearization transformation matrix. , , These are the coefficients of the state iteration equation; The joint state prediction model is matrixed using the following formula: ； in, ; ； ； ； ； The compliance control system also includes: The cost function construction module is used to construct the cost function of the joint state prediction model and add constraints. The cost function analysis module is used to solve the cost function to obtain the main joint position increment; The cost function and constraints are constructed using the following formula: ； ； ； in, The reference angular position is a vector with constant coefficients. S Let P be the coefficient matrix of the cost function, and let P be the angular constraint coefficient matrix. is the angular position vector.

5. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the compliant control method for a master-slave robot as described in any one of claims 1 to 3.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the compliant control method for the master-slave robot as described in any one of claims 1 to 3.

Images