CN118163117A

CN118163117A - Method, system, device, medium, and program product for driving robot joint

Info

Publication number: CN118163117A
Application number: CN202410585653.4A
Authority: CN
Inventors: 岳克双; 须晓锋; 于璇; 杨池
Original assignee: Shanghai Shuzhidao Medical Instrument Co ltd
Current assignee: Shanghai Shuzhidao Medical Instrument Co ltd
Priority date: 2024-05-13
Filing date: 2024-05-13
Publication date: 2024-06-11

Abstract

The present disclosure provides a driving method, system, device, medium and program product of a robot joint, the method comprising: constructing a mapping relation between the joint rotation state and the return angle of the robot; acquiring a target rotation angle and a first actual rotation state of the joint; matching to obtain a first return angle; compensating the target rotation angle by adopting the first return angle to obtain a first driving angle of the motor; the motor is controlled to drive the joint to rotate at a first driving angle. According to the method and the device, the compensation relation of the return angle to the target rotation angle is determined by constructing the mapping relation of different rotation states and the return angle of the joint, so that when the robot actually works, the corresponding return angle can be quickly and accurately obtained by matching according to the actual rotation state of the joint, return compensation and return transition are realized, and the accuracy and stability of joint rotation are improved.

Description

Method, system, device, medium, and program product for driving robot joint

Technical Field

The present disclosure relates to the field of mechanical control technologies, and in particular, to a method, a system, an apparatus, a medium, and a program product for driving a robot joint.

Background

In recent years, minimally invasive surgical robots are increasingly popularized and applied in the aspect of assisting surgeons in minimally invasive surgery, and can provide the advantages of visual operation, comfortable operation process, short learning curve, elimination of hand tremors and the like. The surgical instrument is used as an end execution unit, and various actions such as suturing, shearing, pulling, clamping and the like of tissues of a patient can be completed according to the operation of a doctor main end, so that the movement precision and response speed of the surgical instrument directly influence the final surgical result.

However, since the surgical instrument is directly contacted with the patient, the sterility requirement must be met before the surgical instrument is used, the instrument before the surgical instrument is used needs to be subjected to a strict sterilization process, the process is often subjected to severe conditions such as high temperature, high humidity and the like, meanwhile, the service life of the surgical instrument is low, the surgical instrument belongs to a medical consumable, and therefore, the joint is often inconvenient to install a sensor (such as a position encoder for position detection, a tension sensor for force measurement and the like) at the tail end of the surgical instrument, so that a position feedback sensor exists only in a driving unit part of the surgical instrument, and the surgical instrument lacks a position or a force sensor.

The surgical instrument joint is often connected with a driving motor and the joint through a wire transmission, the length of a transmission wire can be changed along with the change of the wire tension, a transmission error is generated, the transmission error is particularly serious when the joint replacement movement is reverse, the reversing transmission error is called a flexible return difference, and the return difference is related to the wire tension.

In order to facilitate the disassembly and replacement of the surgical instrument, a quick-change device (a design that can be quickly separated or reestablished that a joint of the surgical instrument is connected with a motor shaft of a drive motor, such as a cross-head connector, a straight-head connector and the like) is further designed between the surgical instrument driving unit and the surgical instrument, and a multistage planetary reducer is often also arranged in the surgical instrument driving unit, so that a transmission error exists between the surgical instrument driving motor and the joint of the surgical instrument, and particularly, when the movement direction is changed, the transmission error is generated between the motor shaft of the drive motor and the output end of the reducer shaft and the output end of the quick-change connecting shaft, which is called a rigid return difference.

Because of the existence of the rigid return difference and the flexible return difference of the surgical instrument and the lack of corresponding measuring sensors, the accurate position control of the surgical instrument becomes a difficult problem, and partial operators or research institutions adopt an algorithm compensation method to realize the control.

However, the existing return difference compensation method does not consider various complex situations in the use of an actual robot, which results in inaccurate determination of a return difference value and thus inaccurate joint rotation control of the robot.

Disclosure of Invention

The technical problem to be solved by the present disclosure is to provide a driving method, system, device, medium and program product for a robot joint, in order to overcome the defect that various complex situations in actual robot use are not considered in the return difference compensation method in the prior art, resulting in inaccurate joint rotation control of the robot.

The technical problems are solved by the following technical scheme:

the present disclosure provides a driving method of a robot joint, the driving method including:

constructing a static return difference mapping relation between each rotation state of a joint of the robot and a corresponding return difference angle;

The return angle is used for representing the difference between the actual driving angle of the motor and the actual rotation angle of the joint in the robot; the actual driving angle is calculated based on a first rotation angle of a driving shaft of the motor and an equivalent relation between the driving shaft and a joint shaft of the joint;

acquiring a target rotation angle and a first actual rotation state of the joint;

according to the first actual rotation state and the static return difference mapping relation, a first return difference angle of the joint is obtained in a matching mode;

The target rotation angle is compensated by adopting the first return angle so as to obtain a first driving angle of the motor;

and controlling the motor to drive the joint to rotate at the first driving angle.

Preferably, the step of constructing the static return difference mapping relationship between each rotation state of the joint and the corresponding return difference angle further includes:

and testing to obtain the return difference angles of the joints in different rotation states.

Preferably, the rotation state includes at least one of a joint movement direction of the joint, whether a load is applied to the joint, a load acting direction, a load magnitude, and an initial return angle.

Preferably, the step of obtaining the target rotation angle and the first actual rotation state of the joint further includes:

Controlling the motor to drive the joint to rotate to a maximum angle position or a minimum angle position;

acquiring a second rotation angle of the joint and a third rotation angle of the driving shaft;

and acquiring the initial return difference angle according to the second rotation angle and the third rotation angle.

Preferably, the step of compensating the target rotation angle by using the first return angle to obtain a first driving angle of the motor includes:

When the joint is not loaded and the joint movement direction is positive, calculating the sum of the target rotation angle and the first return difference angle to obtain the first driving angle;

When the joint is not loaded and the joint movement direction is negative, calculating the difference between the target rotation angle and the first return difference angle to obtain the first driving angle;

When a load exists on the joint, the movement direction of the joint is positive, the action direction of the load is negative, and the sum of the load and the friction is smaller than or equal to the maximum driving force of the motor, calculating the sum of the target rotation angle and the first return difference angle to obtain the first driving angle;

the friction force is the comprehensive friction force of the motor driving the joint to rotate;

When a load is placed on the joint, the joint movement direction is positive, the load acting direction is positive, and the load is greater than or equal to the friction force, the target rotation angle is taken as the first driving angle;

When a load exists on the joint, the movement direction of the joint is positive, the action direction of the load is positive, and the load is smaller than the friction force, calculating the sum of the target rotation angle and the first return angle to obtain the first driving angle;

When a load exists on the joint, the movement direction of the joint is negative, the action direction of the load is positive, and the sum of the load and the friction force is smaller than or equal to the maximum driving force of the motor, calculating the difference between the target rotation angle and the first return angle to obtain the first driving angle;

When a load is placed on the joint, the movement direction of the joint is negative, the action direction of the load is negative, and the load is greater than or equal to the friction force, the target rotation angle is taken as the first driving angle;

and when the joint is loaded, the joint movement direction is negative, the load acting direction is negative, and the load is smaller than the friction force, calculating the difference between the target rotation angle and the first return angle to obtain the first driving angle.

Preferably, when the rotation state of the joint changes, the step of controlling the motor to drive the joint to rotate at the first driving angle further includes:

When detecting that the rotation state of the joint changes, acquiring a second actual rotation state of the joint;

According to the second actual rotation state and the static return difference mapping relation, a second return difference angle of the joint is obtained in a matching mode;

based on the first return angle, the second return angle and the return transition bandwidth, obtaining an intermediate return angle at each moment between the start of return transition and the completion of return transition;

the intermediate return angle is adopted to compensate the target rotation angle, so that a second driving angle of the motor at different moments is obtained;

Controlling the motor to drive the joint to rotate at the second driving angle;

And when the intermediate return angle is equal to the second return angle, determining that the return transition is completed, and continuously controlling the motor to rotate based on the second driving angle corresponding to the second return angle so as to drive the joint to rotate.

Preferably, the load is calculated by the current of the motor and/or measured by a tension sensor to obtain an initial angle acquisition module, which is used for acquiring the second rotation angle of the joint and the initial angle acquisition module;

And/or the number of the groups of groups,

The actual rotation angle is measured by a position sensor and/or by a camera.

Preferably, the robot includes at least one of a surgical robot, an industrial robot, and a collaborative robot.

The present disclosure also provides a driving system of a robot joint, the driving system including:

The mapping relation construction module is used for constructing a static return difference mapping relation between each rotation state of the joints of the robot and the corresponding return difference angle;

The first state acquisition module is used for acquiring a target rotation angle and a first actual rotation state of the joint;

The first return difference acquisition module is used for matching and obtaining a first return difference angle of the joint according to the first actual rotation state and the static return difference mapping relation;

the first angle acquisition module is used for compensating the target rotation angle by adopting the first return difference angle so as to obtain a first driving angle of the motor;

And the first driving module is used for controlling the motor to drive the joint to rotate at the first driving angle.

Preferably, the driving system further comprises:

And the return difference testing module is used for testing and obtaining the return difference angles of the joints in different rotation states.

Preferably, the driving system further comprises:

the initial rotation control module is used for controlling the motor to drive the joint to rotate to a maximum angle position or a minimum angle position;

An initial angle acquisition module for acquiring a second rotation angle of the joint and a third rotation angle of the drive shaft;

the initial return difference acquisition module is used for acquiring the initial return difference angle according to the second rotation angle and the third rotation angle.

Preferably, the first angle acquisition module is further configured to:

Preferably, when the rotational state of the joint is changed, the driving system further includes:

the second state acquisition module is used for acquiring a second actual rotation state of the joint;

The second return difference acquisition module is used for matching and obtaining a second return difference angle of the joint according to the second actual rotation state and the static return difference mapping relation;

the third return difference acquisition module is used for determining an intermediate return difference angle at each moment between the time when return difference transition is started and the time when return difference transition is completed based on the first return difference angle, the second return difference angle and the return difference transition bandwidth;

the second angle acquisition module is used for compensating the target rotation angle by adopting the intermediate return difference angle so as to obtain a second driving angle of the motor;

the second driving module is used for controlling the motor to drive the joint to rotate at the second driving angle;

And the transition completion determining module is used for determining that the return difference transition is completed when the intermediate return difference angle is equal to the second return difference angle, and continuously controlling the motor to rotate based on the second driving angle corresponding to the second return difference angle so as to drive the joint to rotate.

Preferably, the load size is calculated by current of the motor and/or measured by a tension sensor;

And/or the number of the groups of groups,

The actual rotation angle is measured by a position sensor and/or by a camera.

The disclosure also provides an electronic device comprising a memory, a processor and a computer program stored on the memory and used for running on the processor, wherein the driving method of the robot joint is realized when the processor executes the computer program.

The present disclosure also provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the above-described driving method of a robot joint.

The present disclosure also provides a computer program product comprising a computer program which, when executed by a processor, implements the method of driving a robotic joint described above.

On the basis of conforming to the common knowledge in the art, the preferred conditions can be arbitrarily combined to obtain the preferred examples of the disclosure.

The positive progress effect of the present disclosure is: the return angle of the robot joint under different rotation states is obtained through pre-testing, the compensation relation of the return angle to the target rotation angle is determined, and a corresponding return transition method is designed, so that when the robot actually works, the corresponding return angle can be quickly and accurately obtained by matching according to the actual rotation state of the joint, and the driving angle of a motor is obtained by calculating, so that return compensation and return transition are realized, the accuracy and stability of the rotation of the robot joint are improved, and the comprehensive control effect of the robot is further improved.

Drawings

Fig. 1 is a first flowchart of a driving method of a robot joint of embodiment 1 of the present disclosure.

Fig. 2 is a second flowchart of a driving method of a robot joint of embodiment 1 of the present disclosure.

Fig. 3 is a schematic diagram of a control principle of a surgical instrument according to embodiment 1 of the present disclosure.

Fig. 4 is a flowchart of a return difference compensation relationship determination method of a surgical instrument of embodiment 1 of the present disclosure.

Fig. 5 is a first block diagram of a driving system of a robot joint according to embodiment 2 of the present disclosure.

Fig. 6 is a second block diagram of the driving system of the robot joint of embodiment 2 of the present disclosure.

Fig. 7 is a schematic structural diagram of an electronic device according to embodiment 3 of the present disclosure.

Detailed Description

The present disclosure is further illustrated by way of examples below, but is not thereby limited to the scope of the examples described.

Prefix words such as "first" and "second" are used in the embodiments of the present disclosure, and are merely for distinguishing between different description objects, and there is no limitation on the location, order, priority, number, content, or the like of the described objects. The use of ordinal words and the like in embodiments of the present disclosure to distinguish between the prefix words describing the object does not limit the described object, and statements of the described object are to be taken in the claims or in the context of the embodiments and should not be construed as unnecessary limitations due to the use of such prefix words. In addition, in the description of the present embodiment, unless otherwise specified, the meaning of "a plurality" is two or more.

In the embodiment of the disclosure, the related processes of collecting, storing, using, processing, transmitting, providing, disclosing and the like of the personal information of the user accord with the rules of related laws and regulations, and do not violate the public order colloquial.

Example 1

The present disclosure provides a driving method of a robot joint, as shown in fig. 1, the driving method including the steps of:

S1, constructing a static return difference mapping relation between each rotation state of a joint of a robot and a corresponding return difference angle;

The return difference angle is used for representing the difference between the actual driving angle of the motor and the actual rotation angle of the joint in the robot; the actual driving angle is calculated based on the first rotation angle of the driving shaft of the motor, the equivalent relationship between the driving shaft and the joint axis of the joint.

When the motor drives the joint to rotate, the actual rotation angle of the joint can only be 5 degrees when the motor drives the joint to rotate by 10 degrees due to the existence of the rigid return difference and the flexible return difference, and the difference between the actual driving angle of the motor and the actual rotation angle of the joint is the return difference angle.

Here, the actual driving angle of the motor means a driving angle equivalent to the motor end to the joint end. In practice, the rotation speed of the driving shaft of the motor is relatively high, the driving shaft of the motor needs to be connected to the joint through a speed reducer, and the joint shaft of the joint can rotate 5 degrees every time the driving shaft of the motor rotates 180 degrees, so that the actual driving angle of the motor is equivalent to the angle from the first rotation angle of the driving shaft of the motor to the joint shaft. The actual driving angle is calculated based on the first rotation angle of the driving shaft of the motor, the equivalent relationship between the driving shaft and the joint shaft.

The return difference angles of the joints in each rotation state can be obtained through testing and the like, and a one-to-one correspondence between each rotation state of the joints and the corresponding return difference angle is constructed, so that a static return difference mapping relation is obtained.

S2, acquiring a target rotation angle and a first actual rotation state of the joint.

When the robot is actually put into use, the joint end of the robot may not be provided with a position encoder or a testing device such as a camera due to the use environment and the like, and then the driving angle of the motor needs to be calculated according to the target rotation angle and the static return difference mapping relation. Thus, step S2 obtains the target rotation angle and the first actual rotation state of the current joint.

And S3, according to the first actual rotation state and the static return difference mapping relation, a first return difference angle of the joint is obtained through matching.

Because the mapping relation is constructed in the step S1, after the actual rotation state of the joint is obtained, the accurate return value in the current rotation state can be obtained by matching.

And S4, compensating the target rotation angle by adopting the first return angle to obtain a first driving angle of the motor.

Since there is a return difference between the driving angle of the motor and the rotation angle of the joint, when it is necessary to control the rotation angle of the joint rotation target, it is necessary to compensate the return difference to determine the driving angle of the motor.

Specifically, according to different rotation states, the method can be divided into calculating the sum of the target rotation angle and the first return difference angle to obtain a first driving angle, calculating the difference between the target rotation angle and the first return difference angle to obtain a first driving angle, and directly taking the target rotation angle as the first driving angle.

S5, controlling the motor to drive the joint to rotate at a first driving angle.

In the scheme, by constructing the mapping relation between different rotation states of the robot joint and corresponding return difference angles and the compensation relation of the return difference angles to the target rotation angles, when the robot actually works, the corresponding return difference angles can be quickly and accurately obtained by matching according to the actual rotation states of the joints, the driving angles of the motors are obtained by calculation, the rigid return difference and the flexible return difference between the motors and the joints are compensated, the accuracy of the rotation of the robot joint is improved, and the control effect of the robot is further improved.

In an embodiment, step S1 further includes:

And testing to obtain return difference angles of the joints of the robot in different rotation states.

The return difference of the robot is affected by the rotation state (such as direction, load, etc.) of the joints of the robot, and the different influencing factors and the combination of influencing factors lead to the complexity of the return difference. The present disclosure contemplates various different rotational conditions, with each return difference value being purposefully obtained through a return difference test.

Specifically, the joints of the robot are controlled to rotate in different rotation states, and corresponding return difference angles are obtained through testing. The actual driving angle of the motor is obtained by testing the rotation angle of the driving shaft through a position encoder at the motor end or a testing device such as a camera and the like and then calculating according to the equivalent relation of the rotation angle between the driving shaft and the joint shaft; the actual rotation angle of the joint is obtained through testing by a position encoder or a camera and other testing devices at the joint end; the return difference angle is the difference between the actual driving angle of the motor and the actual rotation angle of the joint in the same rotation state.

In the scheme, the return angle corresponding to the joint is obtained through the tests under different rotation states, various influencing factors of the return angle can be fully considered, and the accurate return angle under each rotation state is obtained.

In one embodiment, the rotational state includes the articulation direction of the joint, the presence or absence of a load on the joint, the direction of the load applied, the magnitude of the load, and the initial return angle.

The joint movement direction comprises positive direction and negative direction, wherein the positive and negative directions of the movement direction can be customized by an operator according to actual needs.

The direction of load application is divided into the same direction as the articulation direction and opposite the articulation direction.

The presence or absence of load on the joint and the magnitude of the load will affect the length of the drive wire and thus cause variations in backlash, and therefore it is also necessary to consider this in different rotational conditions. The load size can be obtained according to the actual current value of the motor or a tension sensor and the like.

If there are two or more joints in the robot in succession, the effect of the return angle of one joint on the other joint will also be reflected in the direction of load application and in the magnitude of the load.

In the scheme, by fully considering different rotation states such as the joint movement direction, the existence of load on the joint, the load acting direction, the load size, the initial return difference angle and the like and the combination of the rotation states, the corresponding static return difference mapping relation under various states can be accurately and completely constructed, and the accuracy of return difference compensation is improved.

In an embodiment, step S2 further includes:

And acquiring an initial return difference angle according to the second rotation angle and the third rotation angle.

For example, for a detachable robot, after the detachment is completed, there may be a certain initial return angle between the motor and the joint, and at this time, the initial return angle needs to be determined first.

The method for determining the initial return angle can include: the joint is controlled to rotate to a maximum angular position (at a maximum limit position) or to a minimum angular position (at a minimum limit position), at which time a second rotational angle of the joint can be known from the limit position of the joint, wherein the limit position is physically limited, and the joint angle relative to the reference position can be measured in advance by the measuring device. The third rotation angle of the drive shaft can be obtained by a testing device such as a position encoder. According to the third rotation angle of the driving shaft, the driving angle of the motor can be calculated, and according to the driving angle of the motor and the rotation angle of the joint, the initial return angle can be calculated.

In the scheme, the initial return angle between the motor and the joint of the robot is determined, so that the interference caused by factors such as disassembly of the robot can be eliminated, the return angle is determined more accurately, and the joint of the robot is controlled more accurately.

In one embodiment, step S4 includes:

when no load is applied to the joint and the joint movement direction is positive, calculating the sum of the target rotation angle and the first return difference angle to obtain a first driving angle;

When the joint is not loaded and the joint movement direction is negative, calculating the difference between the target rotation angle and the first return difference angle to obtain a first driving angle;

when a load exists on the joint, the movement direction of the joint is positive, the action direction of the load is negative, and the sum of the load and the friction is smaller than or equal to the maximum driving force of the motor, calculating the sum of the target rotation angle and the first return angle to obtain a first driving angle;

The friction force is the comprehensive friction force of the motor driving joint to rotate;

When a load exists on the joint, the joint movement direction is positive, the load acting direction is positive, and the load is greater than or equal to the friction force, the target rotation angle is taken as a first driving angle;

when a load exists on the joint, the joint movement direction is forward, the load acting direction is forward, and the load is smaller than the friction force, calculating the sum of the target rotation angle and the first return angle to obtain a first driving angle;

when a load exists on the joint, the movement direction of the joint is negative, the action direction of the load is positive, and the sum of the load and the friction force is smaller than or equal to the maximum driving force of the motor, calculating the difference between the target rotation angle and the first return angle to obtain a first driving angle;

When the joint is loaded, the joint movement direction is negative, the load acting direction is negative, and the load is greater than or equal to the friction force, the target rotation angle is used as a first driving angle;

The magnitude of the friction force in the above states is the magnitude of the comprehensive friction force generated by the motor driving the joint to rotate, for example, the friction force generated by the processes of motor rotation, speed reducer rotation, wire transmission, joint rotation and the like.

In addition, in the following two rotation states, the motor cannot drive the joint to rotate, so that errors are reported: (1) When the joint is loaded, the joint movement direction is positive, the load acting direction is negative, and the magnitude of the load plus the magnitude of the friction force is larger than the maximum driving force of the motor; (2) When the joint is loaded, the joint movement direction is negative, the load acting direction is positive, and the magnitude of the load plus the magnitude of the friction force is larger than the maximum driving force of the motor.

Besides the states, the rotation state of the motor can be configured in a self-defined mode according to the actual working requirements of the robot.

In the scheme, the first driving angle of the joint can be accurately obtained by defining the compensation relation of the return difference angle to the target rotation angle under each rotation state, and the control precision of the rotation of the joint is improved.

In one embodiment, as shown in fig. 2, step S5 includes:

And S6, when detecting that the rotation state of the joint changes, acquiring a second actual rotation state of the joint.

And S7, matching to obtain a second return difference angle of the joint according to the second actual rotation state and the static return difference mapping relation.

And S8, determining an intermediate return angle at each moment between the transition of the return difference from the beginning to the completion of the return difference transition based on the first return difference angle, the second return difference angle and the return difference transition bandwidth.

The return transition bandwidth is set by an operator according to the control requirement of the actual robot, and generally, the return transition is completed faster and better on the premise of not affecting the working stability of the robot.

For example, the intermediate return angle at each time between the start of the return transition and the completion of the return transition may be determined according to the first return angle, the second return angle, the return transition bandwidth, and a preset return transition model:

Wherein, For the first return angle,/>For the second return angle, f is the return transition bandwidth, t is the return transition time,/>，/>Is an intermediate return angle,/>。

And S9, compensating the target rotation angle by adopting the intermediate return difference angle to obtain a second driving angle of the motor at different moments.

S10, controlling the motor to drive the joint to rotate at a second driving angle.

And S11, when the intermediate return angle is equal to the second return angle, determining that return transition is completed, and continuously controlling the motor to rotate based on the second driving angle corresponding to the second return angle so as to drive the joint to rotate.

At an intermediate return angleFrom/>Transition to/>And when the return difference transition is determined to be completed.

In the scheme, by setting the return difference transition mode, smooth transition between different static return differences can be realized, so that the control stability of the robot is improved.

In one embodiment, the load level is calculated from the current of the motor and/or measured by a tension sensor.

In one embodiment, the actual angle of rotation is measured by a position sensor and/or by a camera.

In the test stage, the joint end can be provided with a sensor and other test devices, so that parameters such as the load size, the actual rotation angle and the like can be obtained through the mode.

Of course, the test modes include, but are not limited to, the above, and can be obtained in other possible manners.

In the scheme, parameters in return difference test are acquired by means of setting a sensor, measuring motor current and the like, so that the return difference value can be tested more accurately, and the accuracy and reliability of subsequent return difference compensation are further ensured.

In one embodiment, the robot comprises a surgical robot, an industrial robot, a collaborative robot.

For example, the robot may comprise a laparoscopic surgical robot.

In the scheme, various robots including the operation robot have stricter requirements on high-temperature, high-humidity and other working environments, the motor driving angle is controlled through the steps of return difference compensation and return difference transition, and the accurate control on the joint rotation angle can be realized under the condition that a sensor is not arranged at the joint end of the robot, so that the working accuracy and stability of the robot are improved, and the comprehensive performance of the robot is improved.

The following describes the principle of implementation of the driving method of the robot joint according to the present embodiment, with reference to a control schematic diagram of the surgical instrument shown in fig. 3.

The embodiment comprises a method for determining return difference compensation relation based on instruction return difference compensation(Including the first return angle described above) command the articulation angle of the surgical instrument/>(The target rotation angle) to obtain a motor shaft rotation angle command/>The method (the first driving angle) corresponds to the specific implementation manner of the step S5.

In particular, it can be based on the motor drive torque during articulation(Equivalent to the representation of the joint end) (maximum driving force of the motor), motor shaft angular velocity/>(Equivalent to the representation of the joint ends), joint stiction moment/>(The magnitude of the friction force described above), external load moment/>The relationship between the magnitude and direction of the load (the magnitude of the load) is used for obtaining a motor shaft rotation angle command/>, which is used for driving the motor to rotate(Equivalent to the representation of the joint ends) and surgical instrument joint rotation angle instructions/>Instruction return difference compensation/>, with respect to joint angleA basic formula of the relationship between them.

Obtaining a motor shaft rotation angle feedback value of motor rotation(Equivalent to the representation of the joint ends) and surgical instrument joint rotation angle feedback values/>Feedback of the difference compensation with respect to the joint angle/>(Equivalent to the representation of the joint ends) basic formula (see FIG. 4),/>, in the respective formulas of FIG. 4、/>、/>、/>、/>、/>The relation between the parameters is only described, and specific values are not represented, and the specific values of the parameters in each formula are determined according to the actually received angle instruction or the return difference value obtained by testing.

Mainly comprises the following steps:

(1) Detecting whether the joint moves in an idle state; if yes, turning to the step (2), and if not, turning to the step (5); the detection method can be realized by feeding back current through a motor;

(2) Detecting whether the joint moves forward; if yes, turning to the step (3), and if not, turning to the step (4);

(3) Obtaining a basic formula of a return difference compensation relation of the forward no-load motion joint:

(4) Obtaining a basic formula of a return difference compensation relation of the negative and positive no-load motion joint:

(5) Detecting whether the joint moves forward, if yes, turning to the step (6), and if not, turning to the step (13); the detection method is based on the rotation angle of the motor Whether greater than 0 or some relatively small positive number is implemented;

(6) Detecting whether the external moment direction is the same as the movement direction, if not, turning to the step (7), and if so, turning to the step (10); the detection method is that Direction and/>Whether or not to be in the same direction;

(7) Detecting whether the external moment and the joint friction moment exceed the set maximum current of the motor, if so, turning to the step (8), and if not, turning to the step (9);

(8) Outputting negative overload error reporting;

(9) The basic formula of the return difference compensation relation of the positive motion of the output motor under the allowable negative external moment (the allowable expression here indicates the external force which the motor can bear):

(10) Detecting whether the external torque is larger than or equal to the joint friction torque, if so, turning to the step (11), and if not, turning to the step (12);

(11) The forward motion of the output motor receives Xu Yongzheng external torque, and the external torque is larger than or equal to the basic formula of the return difference compensation relation of the joint when the static friction torque of the joint is larger than or equal to the external torque:

(12) The forward motion of the output motor is subjected to Xu Yongzheng external moment, and the external moment is smaller than the basic formula of the return difference compensation relation of the joint when the static friction moment of the joint:

(13) Detecting whether the external moment direction is the same as the movement direction, if not, turning to the step (14), and if so, turning to the step (17); the detection method is that Direction and/>Whether or not to be in the same direction;

(14) Detecting whether the external moment and the joint friction moment exceed the set maximum current of the motor, if so, turning to the step (15), and if not, turning to the step (16);

(15) Outputting a forward overload error report;

(16) The basic formula of the return difference compensation relation of the negative movement of the output motor under Xu Yongzheng external moment:

(17) Detecting whether the external torque is larger than or equal to the joint friction torque, if so, turning to the step (18), and if not, turning to the step (19);

(18) The negative movement of the output motor is subjected to allowable negative external moment, and the external moment is larger than or equal to the basic formula of the joint return difference compensation relation when the joint static friction moment:

(19) The negative movement of the output motor is subjected to allowable negative external moment, and the external moment is smaller than the basic formula of the joint return difference compensation relation when the static friction moment of the joint:

The joint static friction moment and return difference values of different positions of the joint can be obtained through a test in advance. The external torque can be obtained through a force sensor of a driving unit part of the surgical instrument, and can also be obtained through a motor current integrated value.

The embodiment also includes a return difference transition method for implementing transition between different static return differences, i.e. a scheme corresponding to the steps S8-S14, mainly including the following steps:

(1) Detecting whether the rotation state of the joint changes, if so, turning to the step (2), and if not, turning to the step (7) without changing the return difference compensation value;

(2) Determining an initial return difference value according to the static return difference mapping relation (The first return difference angle described above) and determining the expected return difference value/>(The second return angle);

(3) Determining a return difference transition compensation bandwidth as f (the return difference transition bandwidth); the method is used for adjusting return difference compensation transition time, and the larger f is, the shorter the return difference transition time is;

(4) The return difference transition algorithm is executed, and the formula can be as follows:

wherein t is the time consumption of return difference transition compensation, ； />For the return difference transition compensation value (the above intermediate return difference angle)/>；

The above-mentioned methodAnd/>Are vectors,/>Representing the difference size between the desired return difference value and the initial return difference value.

The return difference transition compensation method comprises the following steps:

For instructions

For feedback

/>

(5) Detecting whether the return difference transition is finished or not, and if not, turning to the step (5); if yes, turning to the step (6);

(6) And outputting a final return difference compensation value (namely the second return difference angle) after the return difference compensation transition is finished.

The embodiment further includes a method for obtaining the second driving angle of the joint, that is, the schemes corresponding to the steps S2 to S3 and S7 to S13 are adopted to realize the transition from the first return angle to the second return angle. It is based on the joint instruction angleAnd external force/>Information that compensates a joint angle command (the target rotation angle) by using a joint command return difference (the first return difference angle or the second return difference angle) to obtain a compensated joint command/>(The first driving angle) is implemented as follows:

(1) According to the expected joint angle command (the target rotation angle), the expected joint angular velocity command (which can be calculated according to the target rotation angle and the rotation period), the external moment size of the joint, the direction and the friction moment size of the joint, a basic formula of an expected return difference compensation relation is determined, and a joint command after the expected value of the return difference compensation of the joint command (The second return angle);

(2) Determining the expected value of the joint instruction return difference according to a basic formula of the expected joint return difference compensation relation (The second return angle); determining the current value of the joint instruction return difference according to the basic formula of the current joint return difference compensation relation(The first return angle);

(3) According to the return difference transition method, the joint instruction return difference compensation value is determined ; After the transition is completed, the transition point is changed,The current joint static state return difference value is updated to a transitional state;

(4) Determining a third driving angle of the compensated joint according to the joint command return difference compensation value (the second return difference angle) It is also a command value for the motor shaft joint angle (the above second driving angle) equivalent to the joint end.

The embodiment further includes a method for obtaining the joint rotation angle, which is a feedback difference compensation model of the joint angle, and is used for estimating the rotation angle of the joint according to the rotation angle of the driving shaft of the motor. It is based on the equivalent feedback angle of the motor(Actual driving angle of motor), motor equivalent feedback angular velocity/>(Obtained from the actual drive angle and the control period of the motor) and external force/>(Load size) information, feedback value/>, of equivalent joint angle of motor shaftFeedback of difference compensation by joint angle/>(Return angle) to obtain joint angle feedback estimation value/>The specific implementation steps are as follows:

(1) Determining a static base of a feedback difference of the expected joint angle and an expected value of the feedback difference of the joint angle according to the motor equivalent feedback angle, the motor equivalent feedback angular speed, the joint external torque, the joint friction torque and the joint friction torque Namely, determining a second return difference angle according to the static return difference mapping relation;

(2) Determining a joint angle feedback difference expected value according to the expected joint angle feedback difference static base; determining the initial value of the feedback difference of the previous joint according to the static base of the feedback difference of the previous joint (Initial value), namely determining a first return difference angle according to the static return difference mapping relation;

(3) Determining a joint angle feedback return difference compensation value according to the return difference transition model (I.e., an intermediate return angle); after the transition is completed, update/>The previous joint angle feedback difference static base is updated to a transitional state;

(4) Determining a joint angle feedback estimated value according to the joint angle feedback difference compensation value Namely, estimating the actual rotation angle of the joint according to the actual driving angle and the intermediate return angle of the motor;

The joint feedback difference compensation model is an algorithm basis for joint feedback difference identification, and a tool can be designed to acquire the actual rotation angle of a joint in other non-contact modes such as vision The basic formula of the joint return difference compensation relation under different conditions and the rotation angle/>, fed back by a motor end encoder, of a driving shaft of a motor are utilizedAnd a reduction ratio N, can be obtained, and respectively identify return difference values/>, under different conditionsIn determining the first drive angle of the motor, let/>In estimating the actual rotation angle of the joint, let/>= />。

The embodiment comprises a drive control frame for a robot joint. The frame splits joint instructions (the first driving angle) and joint feedback (the actual rotation angle of a joint) and respectively carries out return difference compensation according to the joint instructions (the target rotation angle) and motor angle feedback (the actual driving angle of a motor), so that the joint instructions are not directly used for motor shaft control, the motor shaft feedback is not directly used as joint end feedback, the influence of joint return difference (the return difference angle) is fully considered, the upper joint instructions are considered to obtain motor shaft end instructions (the first driving angle) after compensation to realize motor shaft control, the motor shaft feedback obtains joint angle feedback values (the estimated actual rotation angle of the joint) of the joint end after compensation, the correspondence of the joint instructions and the joint feedback is realized, and the response of the joint return difference state change and the control precision after the joint return difference state change are improved. Mainly comprises the following steps or links:

(1) Obtaining a joint angle instruction according to an upper control algorithm ；

(2) According toObtaining the angular velocity instruction/>, of the joint by utilizing a differential algorithm; S in fig. 3 is a differential element in the automatic control field; the calculation method comprises the following steps:

In the method, in the process of the invention, The angle instruction is the joint angle instruction of the previous period; ts is the control period;

(3) According to External moment/>The sign and the magnitude of the joint, and the magnitude of the joint static friction moment, and the basic formula (shown in figure 4) of the return difference compensation relation of the joint is utilized to obtain the compensated joint instruction/>；

(4) Calculating a motor shaft rotation angle command (the second driving angle) according to the reduction ratioWherein N is a reduction ratio, and comprises a reduction ratio of a speed reducer and a reduction ratio of a wire transmission part,/>The motor shaft end rotation angle instruction value;

(5) According to the instruction value of the rotation angle of the motor shaft end And a motor shaft end rotation angle feedback value/>, which is obtained according to feedback of a motor end position encoderAnd motor current collected by the driver/>The motor is driven to rotate by utilizing a three-ring control algorithm in the driver;

(6) Feedback value of motor shaft end rotation angle obtained according to feedback of motor end position encoder And a reduction ratio N, calculating a motor shaft joint angle feedback value equivalent to a joint end/>；

(7) According to the motor shaft joint angle feedback value equivalent to the joint endObtaining the feedback value of the angular speed of the motor shaft by utilizing a differential algorithm

In the method, in the process of the invention,The feedback value of the joint angle of the motor shaft equivalent to the joint end in the previous period; ts is the control period;

(8) According to the motor shaft joint angle feedback value equivalent to the joint end Calculating the joint angle feedback estimated value/>, by using the joint feedback difference compensation model；

Wherein the external momentThe magnitude and direction (sign) of (a) can be directly measured based on the addition of a tension sensor or a miniature torque sensor at the power box, and can also be obtained through motor current estimation.

The joint static friction moment can be obtained through a static friction moment test (a mature algorithm process) when no load exists.

The effect of this specific embodiment is that: the method provides a complete static return difference mapping relation and a compensation relation of joint return difference, and realizes smooth transition of different return difference angles through a return difference transition method. The method specifically comprises the following steps: the first driving angle of the motor is obtained through compensation through the target rotation angle and the return difference angle of the joint so as to drive the joint to rotate; and estimating the rotation angle of the motor through the rotation angle and the return difference angle of the driving shaft of the motor. Compared with the traditional neural network algorithm and the hysteresis curve compensation model, the parameter identification of the method is more complete and scientific, and the instruction compensation is controllable and safe.

According to the driving method of the robot joint, the return angle of the robot joint in different rotation states is obtained through pre-testing, the compensation relation of the return angle to the target rotation angle is determined, and the corresponding return transition method is designed, so that when the robot actually works, the corresponding return angle can be quickly and accurately obtained through matching according to the actual rotation state of the joint, the driving angle of the motor is obtained through calculation, return compensation and return transition are achieved, the accuracy and stability of the rotation of the robot joint are improved, and the comprehensive control effect of the robot is further improved.

Example 2

The present embodiment provides a driving system for a robot joint, as shown in fig. 5, including:

the mapping relation construction module 1 is used for constructing a static return difference mapping relation between each rotation state of a joint of the robot and a corresponding return difference angle;

the return difference angle is used for representing the difference between the actual driving angle of the motor and the actual rotation angle of the joint in the robot; the actual driving angle is calculated based on the first rotation angle of the driving shaft of the motor and the equivalent relation between the driving shaft and the joint shaft of the joint;

a first state acquisition module 2 for acquiring a target rotation angle and a first actual rotation state of the joint;

the first return difference acquisition module 3 is used for matching to obtain a first return difference angle of the joint according to the first actual rotation state and the static return difference mapping relation;

The first angle acquisition module 4 is used for compensating the target rotation angle by adopting the first return difference angle so as to obtain a first driving angle of the motor;

the first driving module 5 is used for controlling the motor to drive the joint to rotate at a first driving angle.

In one embodiment, as shown in fig. 6, the drive system further comprises:

And the return difference testing module 6 is used for testing and obtaining return difference angles of the joint in different rotation states.

In one embodiment, the drive system further comprises:

an initial rotation control module 7 for controlling the motor to drive the joint to rotate to a maximum angular position or a minimum angular position;

An initial angle acquisition module 8 for acquiring a second rotation angle of the joint and a third rotation angle of the drive shaft;

The initial return difference obtaining module 9 is configured to obtain an initial return difference angle according to the second rotation angle and the third rotation angle.

In an embodiment, the first angle acquisition module 4 is further configured to:

In one embodiment, when the rotational state of the joint changes, as shown in fig. 6, the driving system further includes:

a second state acquisition module 10, configured to acquire a second actual rotation state of the joint when detecting that the rotation state of the joint changes;

the second return difference obtaining module 11 is configured to obtain a second return difference angle of the joint by matching according to a second actual rotation state and a static return difference mapping relationship;

a third return difference obtaining module 12, configured to obtain, based on the first return difference angle, the second return difference angle, and the return difference transition bandwidth, an intermediate return difference angle that determines each time between when the return difference transition is started and when the return difference transition is completed;

A second angle obtaining module 13, configured to compensate the target rotation angle by using the intermediate return angle, so as to obtain a second driving angle of the motor at different moments;

The second driving module 14 is used for controlling the motor to drive the joint to rotate at a second driving angle;

And the transition completion determining module 15 is configured to determine that the return difference transition is completed when the intermediate return difference angle is equal to the second return difference angle, and continuously control the motor to rotate based on the second driving angle corresponding to the second return difference angle, so as to drive the joint to rotate.

In one embodiment, the load is calculated from the current of the motor and/or measured by a tension sensor;

For system embodiments, reference is made to the description of method embodiments for the relevant points, since they essentially correspond to the method embodiments. The system embodiments described above are merely illustrative, in which elements illustrated as separate elements may or may not be physically separate, and elements illustrated as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the objectives of the disclosed solution.

According to the driving system of the robot joint, the return angle of the robot joint in different rotation states is obtained through pre-testing, the compensation relation of the return angle to the target rotation angle is determined, and a corresponding return transition method is designed, so that when the robot actually works, the corresponding return angle can be quickly and accurately obtained through matching according to the actual rotation state of the joint, the driving angle of a motor is obtained through calculation, return compensation and return transition are achieved, the accuracy and stability of the rotation of the robot joint are improved, and the comprehensive control effect of the robot is further improved.

Example 3

The embodiment provides an electronic device, and fig. 7 is a schematic block diagram of the electronic device. The electronic device includes a memory, a processor, and a computer program stored on the memory and executable on the processor, and the processor implements the driving method of the robot joint of embodiment 1 when executing the program. The electronic device 30 shown in fig. 7 is merely an example and should not be construed to limit the functionality and scope of use of the disclosed embodiments.

As shown in fig. 7, the electronic device 30 may be embodied in the form of a general purpose computing device, which may be a server device, for example. Components of electronic device 30 may include, but are not limited to: the at least one processor 31, the at least one memory 32, a bus 33 connecting the different system components, including the memory 32 and the processor 31.

The bus 33 includes a data bus, an address bus, and a control bus.

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.

Memory 32 may also include a program/utility 325 having a set (at least one) of program modules 324, such 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 which may include an implementation of a network environment.

The processor 31 executes various functional applications and data processing, such as a driving method of the robot joint of embodiment 1 of the present disclosure, by running a computer program stored in the memory 32.

The electronic device 30 may also communicate with one or more external devices 34 (e.g., keyboard, pointing device, etc.). Such communication may be through an input/output (I/O) interface 35. Also, model-generating device 30 may also communicate with one or more networks, such as a Local Area Network (LAN), a Wide Area Network (WAN), and/or a public network, such as the internet, via network adapter 36. As shown in fig. 7, network adapter 36 communicates with the other modules of model-generating device 30 via bus 33. It should be appreciated that although not shown in the figures, other hardware and/or software modules may be used in connection with the model-generating device 30, including, but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, data backup storage systems, and the like.

It should be noted that although several units/modules or sub-units/modules of an electronic device are mentioned in the above detailed description, such a division is merely exemplary and not mandatory. Indeed, the features and functionality of two or more units/modules described above may be embodied in one unit/module in accordance with embodiments of the present disclosure. Conversely, the features and functions of one unit/module described above may be further divided into ones that are embodied by a plurality of units/modules.

Example 4

The present embodiment provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the driving method of the robot joint of embodiment 1.

More specifically, among others, readable storage media may be employed 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 of the foregoing.

In a possible embodiment, the disclosure may also be implemented in the form of a program product comprising program code for causing a terminal device to carry out a driving method of a robotic joint implementing embodiment 1, when the program product is run on the terminal device.

Wherein the program code for carrying out the present disclosure may be written in any combination of one or more programming languages, and the program code may execute entirely on the user device, partly on the user device, as a stand-alone software package, partly on the user device, partly on a remote device or entirely on the remote device.

Example 5

The embodiment of the present disclosure also provides a computer program product, including a computer program, which when executed by a processor implements the driving method of the robot joint in the above embodiment 1.

Wherein the program code for carrying out the computer program product of the present disclosure may be written in any combination of one or more programming languages, the program code may be executed entirely on the user device, partly on the user device, as a stand-alone software package, partly on the user device and partly on a remote device or entirely on the remote device.

While specific embodiments of the present disclosure have been described above, it will be appreciated by those skilled in the art that this is by way of example only, and the scope of the disclosure is defined by the appended claims. Various changes and modifications to these embodiments may be made by those skilled in the art without departing from the principles and spirit of the disclosure, but such changes and modifications fall within the scope of the disclosure.

Claims

1. A driving method of a robot joint, the driving method comprising:

2. The method of driving a robot joint according to claim 1, wherein the step of constructing a static return difference map between each rotational state of the robot joint and a corresponding return difference angle further comprises, prior to:

3. The method of driving a robot joint according to claim 2, wherein the rotation state includes at least one of a joint movement direction of the joint, whether a load is applied to the joint, a load acting direction, a load magnitude, and an initial backlash angle.

4. The method of driving a robot joint according to claim 3, wherein the step of acquiring the target rotation angle and the first actual rotation state of the joint further comprises, before:

5. The method of driving a robot joint according to claim 3, wherein the step of compensating the target rotation angle using the first return angle to obtain the first driving angle of the motor comprises:

6. The method of driving a robotic joint as set forth in any one of claims 1-5, wherein the step of controlling the motor to drive the joint to rotate at the first drive angle further comprises, after:

Controlling the motor to drive the joint to rotate at the second driving angle;

7. The method of driving a robotic joint as claimed in any one of claims 3-5, wherein the load magnitude is calculated from the current of the motor and/or measured by a tension sensor;

And/or the number of the groups of groups,

The actual rotation angle is measured by a position sensor and/or by a camera.

8. The method of driving a robot joint according to any one of claims 1 to 5, wherein the robot comprises at least one of a surgical robot, an industrial robot, and a cooperative robot.

9. A drive system for a robotic joint, the drive system comprising:

10. The drive system of a robotic joint as set forth in claim 9, wherein said drive system further comprises:

11. The robot joint driving system according to claim 10, wherein the rotation state includes at least one of a joint movement direction of the joint, whether a load is applied to the joint, a load acting direction, a load magnitude, and an initial backlash angle.

12. The robotic joint drive system according to claim 11, wherein the drive system further comprises:

13. The robotic joint drive system according to claim 11, wherein the first angle acquisition module is further configured to:

14. The drive system of a robotic joint as claimed in any one of claims 9-13, wherein the drive system further comprises:

the second state acquisition module is used for acquiring a second actual rotation state of the joint when detecting that the rotation state of the joint changes;

The third return difference acquisition module is used for acquiring an intermediate return difference angle at each moment between the transition of the return difference from the beginning and the completion of the return difference transition based on the first return difference angle, the second return difference angle and the return difference transition bandwidth;

the second angle acquisition module is used for compensating the target rotation angle by adopting the intermediate return difference angle so as to obtain a second driving angle of the motor at different moments;

15. The drive system of a robotic joint as claimed in any one of claims 11-13, wherein the load magnitude is calculated from the current of the motor and/or measured by a tension sensor;

And/or the number of the groups of groups,

The actual rotation angle is measured by a position sensor and/or by a camera.

16. The drive system of a robotic joint as claimed in any one of claims 9-13, wherein the robot comprises at least one of a surgical robot, an industrial robot, a collaborative robot.

17. An electronic device comprising a memory, a processor and a computer program stored on the memory for execution on the processor, characterized in that the processor implements the method of driving a robotic joint according to any one of claims 1-8 when executing the computer program.

18. A computer-readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, implements the method of driving a robotic joint according to any one of claims 1-8.

19. A computer program product comprising a computer program, characterized in that the computer program, when executed by a processor, implements the method of driving a robot joint according to any one of claims 1-8.