CN116340739A

CN116340739A - Method and device for detecting contact force of instrument, computer equipment and storage medium

Info

Publication number: CN116340739A
Application number: CN202310294803.1A
Authority: CN
Inventors: 请求不公布姓名; 金路凯; 王家寅
Original assignee: Shanghai Microport Medbot Group Co Ltd
Current assignee: Shanghai Microport Medbot Group Co Ltd
Priority date: 2023-03-23
Filing date: 2023-03-23
Publication date: 2023-06-27

Abstract

The present application relates to a method, apparatus, computer device, storage medium and computer program product for detecting an instrument contact force. The method comprises the following steps: acquiring joint parameters corresponding to each joint of a target instrument in a current period; the joint parameters include an angle value and a velocity value of the joint. And determining a target detection mode from a plurality of preset detection modes. And determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint. And determining a numerical value which corresponds to the external torque and is used for reflecting the force according to the mapping relation between the torque and the force, and taking the determined numerical value as the value of the contact force of the tail end of the target instrument in the current period. In this way, the accuracy of contact force detection is improved.

Description

Method and device for detecting contact force of instrument, computer equipment and storage medium

Technical Field

The present application relates to the field of detection technology, and in particular, to a method, an apparatus, a computer device, a storage medium, and a computer program product for detecting a contact force of an instrument.

Background

Along with the development of instrument technology, in the process of carrying out abnormal repair on a target object, in order to facilitate operators to accurately repair the repair part of the target object, the operators can remotely operate the mechanical arm so that the instrument arranged on the mechanical arm repairs the repair part. During the repair process, the instrument may come into contact with the repair site, thereby generating a contact force. In order to avoid damage to the repair site due to excessive contact force, the contact force needs to be detected.

In the conventional art, the contact force is often determined by a tamper-evident card or an image observation instrument. However, in the process of determining the contact force through the stamping card, the measured contact force cannot be directly obtained, the contact force can be determined through a long force transmission chain, and detection deviation is easy to generate, so that the detection precision is not high, namely the problem of low precision of detecting the contact force of the instrument exists.

Disclosure of Invention

In view of the foregoing, it is desirable to provide a method, an apparatus, a computer device, a computer-readable storage medium, and a computer program product for detecting an instrument contact force that can improve the accuracy of the detection of the instrument contact force.

In a first aspect, the present application provides a method of detecting an instrument contact force. The method comprises the following steps:

acquiring joint parameters corresponding to each joint of a target instrument in a current period; the joint parameters comprise an angle value and a speed value of the joint;

determining a target detection mode from a plurality of preset detection modes;

according to the joint parameters corresponding to the joints and the target detection mode, determining the external moment of the target instrument in the current period;

and determining a numerical value which corresponds to the external torque and is used for reflecting the force according to the mapping relation between the torque and the force, and taking the determined numerical value as the value of the contact force of the tail end of the target instrument in the current period.

In a second aspect, the present application also provides a device for detecting a contact force of an instrument. The device comprises:

the parameter acquisition module is used for acquiring joint parameters corresponding to each joint of the target instrument in the current period; the joint parameters comprise an angle value and a speed value of the joint;

the detection mode determining module is used for determining a target detection mode from a plurality of preset detection modes;

the external moment determining module is used for determining the external moment of the target instrument in the current period according to the joint parameters corresponding to each joint and the target detection mode;

and the contact force determining module is used for determining a numerical value which corresponds to the external torque and is used for reflecting the force according to the mapping relation between the torque and the force, and taking the determined numerical value as the value of the contact force of the tail end of the target instrument in the current period.

In a third aspect, the present application also provides a computer device. The computer device comprises a memory storing a computer program and a processor which when executing the computer program performs the steps of:

Determining a target detection mode from a plurality of preset detection modes;

In a fourth aspect, the present application also provides a computer-readable storage medium. The computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps of:

determining a target detection mode from a plurality of preset detection modes;

In a fifth aspect, the present application also provides a computer program product. The computer program product comprises a computer program which, when executed by a processor, implements the steps of:

determining a target detection mode from a plurality of preset detection modes;

The method, the device, the computer equipment, the storage medium and the computer program product for detecting the contact force of the instrument are realized by acquiring the joint parameters corresponding to the joints of the target instrument in the current period; the joint parameters include an angle value and a velocity value of the joint. And determining a target detection mode from a plurality of preset detection modes. And determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint. That is, the external moment can be directly determined by a preset target detection mode through the joint parameters corresponding to the joints in the current period, and errors caused by indirect measurement can be effectively avoided. According to the mapping relation between the moment and the force, the numerical value which corresponds to the external moment and is used for reflecting the force can be rapidly and accurately determined, and the determined numerical value is used as the value of the contact force of the tail end of the target instrument in the current period. Based on the method, through the joint parameters corresponding to the joints of the target instrument in the current period, the contact force generated by the contact between the target instrument and the repair part can be directly and accurately detected by utilizing the target detection mode, and the measurement error generated by indirect measurement is effectively avoided, so that the precision of instrument contact force detection is improved.

Drawings

FIG. 1 is a diagram of an application environment for a method of detecting an instrument contact force in one embodiment;

FIG. 2 is a flow chart of a method of detecting an instrument contact force in one embodiment;

FIG. 3 is a schematic view of a target instrument configuration in one embodiment;

FIG. 4 is a flow chart of determining external torque in one embodiment;

FIG. 5 is a schematic diagram of a wire drive in one embodiment;

FIG. 6 is a schematic diagram of the current versus torque mapping in one embodiment;

FIG. 7 is a flow diagram of determining an operating force in one embodiment;

FIG. 8 is a flow chart of determining external torque in another embodiment;

FIG. 9 is a schematic diagram of a sensor structure in one embodiment;

FIG. 10 is a schematic diagram of sensor installation in one embodiment;

FIG. 11 is a flow chart of determining an operating force in another embodiment;

FIG. 12 is a flow chart of determining an operating force in another embodiment;

FIG. 13 is a schematic flow diagram of a correction in one embodiment;

FIG. 14 is a schematic view of the location of the distal end of a target instrument in one embodiment;

FIG. 15 is a flow chart illustrating a step of determining a target torque in one embodiment;

FIG. 16 is a schematic view of a target instrument in one embodiment;

FIG. 17 is a flowchart illustrating a step of determining a target torque according to another embodiment;

FIG. 18 is a schematic view of a structure of an operation table in one embodiment;

FIG. 19 is a schematic diagram of a display device in one embodiment;

FIG. 20 is a block diagram of a device for detecting an instrument contact force in one embodiment;

fig. 21 is an internal structural view of a computer device in one embodiment.

Detailed Description

In order to make the objects, technical solutions and advantages of the present application more apparent, the present application will be further described in detail with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the present application.

The method for detecting the contact force of the instrument, provided by the embodiment of the application, can be applied to an application environment shown in fig. 1. The application scene includes an operation table 100 for an operator to operate, a repair work vehicle 200 for a robot to repair a target object, an image vehicle 300 for displaying an image, a tool vehicle 400 for placing a repair tool, and an auxiliary member 500 for placing an auxiliary tool. The operation table 100 is provided with a main manipulator, and the repair workbench 200 comprises at least two mechanical arms 201, and instruments and endoscopes are respectively hung on the mechanical arms 201. The repair work carriage 200 communicates with the computer device 600 through a network, the repair work carriage 200 communicates with the operation table 100 through a network (not shown in fig. 1), and the repair work carriage 200 communicates with the image carriage 300 through a network (not shown in fig. 1). The computer device 600 may be a server or a terminal. The data storage system may store data that the server needs to process. The data storage system may be integrated on a server or may be placed on a cloud or other network server.

In some embodiments, the master manipulator is in a master-slave control relationship with the robotic arm 201, as is the master manipulator. Alternatively, the operator performs the abnormality repairing operation on the target object by operating the table 100 and the main operator remote operation. Illustratively, the robotic arm 201 and instrument are moved in accordance with the movement of the main operator in the abnormality repair operation, i.e., in accordance with the operation of the operator's hand. Further, if a contact force is generated between the repair part and the instrument, the main manipulator receives acting force information of the repair part on the instrument and feeds back the acting force information to the hands of the operator, so that the operator can feel abnormal repair operation more intuitively. To facilitate the repair operations by the repair personnel, the console 100 has a display screen that is communicatively coupled to the endoscope mounted on the robotic arm of the repair trolley 200 so that the display device can receive images captured by the endoscope. The operator controls the mechanical arm and the instrument to move by the main control hand according to the image displayed on the display device on the workbench 100 in the draft, so that the endoscope and the instrument can reach the repair part to perform the abnormal repair.

In other embodiments, once the operator cannot complete the corresponding operation by the main operator or an abnormality occurs in the abnormality repairing operation, the intervention of the operator, that is, the intervention operation by the image trolley 300 is required.

In some embodiments, the computer device 600 obtains acquisition data acquired by the target instrument in the repair trolley 200 and determines joint parameters corresponding to each joint of the target instrument in the current cycle based on the acquisition data. The computer device 600 obtains the joint parameters corresponding to the joints of the target instrument in the current period; the joint parameters include an angle value and a velocity value of the joint. The computer device 600 determines the target detection mode from among a plurality of detection modes preset. And determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint. The computer device 600 determines a value corresponding to the external force moment and reflecting the magnitude of the force according to the mapping relationship between the moment and the force, and takes the determined value as the value of the contact force of the tip of the target instrument in the current cycle.

The terminal may be, but is not limited to, various personal computers, notebook computers, smart phones, tablet computers, and portable wearable devices. The portable wearable device may be a smart watch, smart bracelet, headset, or the like. The server may be implemented as a stand-alone server or as a server cluster composed of a plurality of servers.

In one embodiment, as shown in fig. 2, a method for detecting a mechanical contact force is provided, and the method is applied to the computer device in fig. 1 for illustration, and includes the following steps:

step S202, acquiring joint parameters corresponding to each joint of a target instrument in a current period; the joint parameters include an angle value and a velocity value of the joint.

The target instrument is an instrument for performing an abnormality repairing operation, and includes a plurality of joints, and the target instrument may be a continuum instrument or a non-continuum instrument, which is not particularly limited. Because the continuum instrument has a plurality of degrees of freedom and high flexibility relative to the non-continuum instrument, the contact force of the continuum instrument is more difficult to confirm based on the degree of freedom, and the accuracy of the contact force of the ordinary instrument (namely the non-continuum instrument) is lower and the contact force of the continuum instrument is difficult to accurately detect relative to the prior art. A block diagram of a continuum instrument is shown in fig. 3, which is a schematic diagram of the structure of a target instrument in one embodiment. In fig. 3, the joints near the end of the target instrument include pitch joints, autorotation joints, yaw joints, and for the movement of the end of the target instrument, translational movements along the three directions of x, y, and z and pose movements including pitch movements, autorotation movements, and yaw movements of the end of the target instrument. The target instrument is controlled by motor traction to realize the movement of the tail end through a plurality of continuous joints. The pitching joint is controlled by a motor driving the guide wire to perform pitching motion; the autorotation joint is controlled by driving the guide wire by another motor so as to perform autorotation motion; the deflection joint is controlled by two motors to drive two deflection sheets respectively so as to perform deflection movement.

Optionally, the computer device obtains respective joint parameters corresponding to each joint of the target instrument in the current cycle, the joints including a joint near the end of the target instrument and a joint not near the end of the target instrument.

For each joint, the data acquisition device on the motor directly driving the joint to be aimed at acquires the joint parameters of the joint to be aimed at, and the computer device directly acquires the joint parameters sent by the data acquisition device corresponding to the joint to be aimed at.

Further, since the joint parameters acquired by the data acquisition device may have errors, in order to ensure the accuracy of detection, parameter correction may be performed on the joint parameters acquired by the data acquisition device, based on this, for each joint, for example, the data acquisition device on the motor of the targeted joint is directly determined to acquire measurement data of the targeted joint, and the computer device acquires measurement data of the targeted joint and performs data correction on the measurement data to obtain the joint parameters of the targeted joint. Wherein the measurement data comprise joint parameters to be corrected.

In step S204, a target detection mode is determined from a plurality of preset detection modes.

The detection mode is used for determining the external moment of the target instrument, so that the contact force generated when the target instrument is contacted with the repair part is determined based on the external moment. The external moment is a physical quantity for enabling the target instrument to rotate, and external force can be directly determined based on the external moment, and the external force in the application is the contact force.

The detection modes include a first detection mode using a wire drive principle, a second detection mode using a stress sensor, and a third detection mode using joint impulses.

Optionally, the computer device determines the target detection mode from a preset plurality of detection modes different from each other.

Illustratively, the computer device determines the target detection mode of the previous cycle, and directly uses the target detection mode of the previous cycle as the target detection mode of the current cycle.

The computer device determines the target detection mode from a plurality of preset detection modes according to the requirement of the current period.

The first detection method, the second detection method, and the third detection method do not involve obtaining a long force transmission chain, nor determining the degree of deformation. Based on this, any one of the preset detection modes related to the application can directly acquire the external moment, that is, directly determine the contact force based on the external moment.

Step S206, determining the external moment of the target instrument in the current period according to the joint parameters corresponding to the joints and the target detection mode.

As previously mentioned, the external torque is a torque that can cause the target instrument to rotate. The external moment comprises the sub-moment corresponding to each joint, and the mathematical expression form can be regarded as a vector, and the vector comprises the values of the sub-moments corresponding to each joint.

Optionally, when the target detection mode is a first detection mode using a wire transmission principle or a second detection mode using a stress sensor, the computer device determines a theoretical moment and an actual moment according to joint parameters corresponding to each joint respectively by using the target detection mode; the computer device determines an external torque of the target instrument in the current cycle based on the difference between the theoretical torque and the actual torque.

When the target instrument contacts the repair site, if the target instrument does not generate a contact force with the repair site or the contact force is extremely small, the theoretical torque and the actual torque are the same when the target instrument and the repair site are not affected. Once the contact force is generated, or the contact force that causes the influence is generated, there is a difference between the theoretical moment and the actual moment, based on which the external moment is calculated.

When the target detection mode is a third detection mode utilizing joint impulse, the computer equipment determines the external moment of the target instrument in the current period through an impulse model based on joint parameters corresponding to all joints respectively.

For example, when the target detection mode is a first detection mode using the wire drive principle or a second detection mode using the stress sensor, for each joint, the computer device determines a theoretical moment and a real moment corresponding to the joint in question based on the joint parameters of the joint in question, using the target detection mode, and calculates a difference between the theoretical moment and the real moment corresponding to the joint in question. The computer equipment determines the external moment of the target instrument in the current period based on the difference value corresponding to each joint.

When the target detection mode is a third detection mode utilizing joint impulse, the computer equipment acquires an impulse model constructed based on the joint impulse, and determines the external moment of the target instrument in the current period through the impulse model based on joint parameters corresponding to the joints respectively.

And step S208, according to the mapping relation between the moment and the force, determining a numerical value which corresponds to the external moment and is used for reflecting the force, and taking the determined numerical value as the value of the contact force of the tail end of the target instrument in the current period.

Optionally, the computer device obtains a mapping relation between the moment and the force, and determines a numerical value corresponding to the external moment and used for reflecting the magnitude of the force according to the mapping relation between the moment and the external force. The computer device takes the determined value as the value of the contact force of the tip of the target instrument during the current cycle.

The force-moment mapping can be considered as a jacobian matrix for determining the contact force, which represents a matrix from cartesian space to joint space corresponding to the distal end of the target instrument.

As previously mentioned, the external moment may be considered as a vector that includes the sub-external moment for each joint, respectively.

Illustratively, the computer device obtains a jacobian matrix for determining the contact force, and for an inverse of a transpose of the jacobian matrix, the computer device determines a value corresponding to the external torque and reflecting the magnitude of the force based on the inverse and the external torque.

For example, the computer device determines the Jacobian matrix J and the external moment τ for determining the contact force _ext Thereafter, the contact force f is determined based on the following formula _ext ：

f _ext ＝(J ^T ) ^-1 *τ _ext

In the method for detecting the contact force of the instrument, the joint parameters corresponding to the joints of the target instrument in the current period are obtained; the joint parameters include an angle value and a velocity value of the joint. And determining a target detection mode from a plurality of preset detection modes. And determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint. That is, the external moment can be directly determined by a preset target detection mode through the joint parameters corresponding to the joints in the current period, and errors caused by indirect measurement can be effectively avoided. According to the mapping relation between the moment and the force, the numerical value which corresponds to the external moment and is used for reflecting the force can be rapidly and accurately determined, and the determined numerical value is used as the value of the contact force of the tail end of the target instrument in the current period. Based on the method, through the joint parameters corresponding to the joints of the target instrument in the current period, the contact force generated by the contact between the target instrument and the repair part can be directly and accurately detected by utilizing the target detection mode, and the measurement error generated by indirect measurement is effectively avoided, so that the precision of instrument contact force detection is improved.

In some embodiments, as shown in FIG. 4, a flow chart of determining the external torque in one embodiment is shown. The joints comprise a first joint close to the tail end of the target instrument and a second joint not close to the tail end of the target instrument, and the driving mode of the second joint is a wire transmission mode; according to the joint parameters and the target detection mode corresponding to each joint, determining the external moment of the target instrument in the current period comprises the following steps:

step S402, according to joint parameters corresponding to the first joints, determining theoretical moments corresponding to the first joints respectively through a dynamic model corresponding to a non-wire transmission mode.

As described above, the joints of the target device include a pitch joint, a rotation joint, and a yaw joint, i.e., the first joint may be a pitch joint, a rotation joint, or a yaw joint. For the second joint which is not close to the tail end of the target instrument, the driving mode is a wire transmission mode, wherein two types of joints exist in the wire transmission mode, namely, a main joint directly driven by a motor, and a slave joint without motor driving is driven by a wire wheel corresponding to the main joint. As shown in fig. 5, a schematic diagram of a wire drive in one embodiment is shown. Wherein, the wire wheel M is directly connected with the motor, r _m For the radius of the wire wheel M, A _m Is a motor inertia matrix. The wire wheel M is connected with the wire wheel 1 through a guide wire, the wire wheel 2 is connected with the wire wheel 1 through a guide wire, namely, the motor drives the wire wheel M to rotate, and the rotation of the wire wheel M drives the rotation of the wire wheel 1 and the wire wheel 2. The joints mentioned in the present application are main joints, pitch joints, autorotation joints, and yaw joints directly determined by the motors. The primary joint is the second joint.

The dynamics model corresponding to the non-wire drive mode refers to a basic dynamics model, namely a model which does not consider the wire drive condition.

Optionally, the computer device obtains joint parameters corresponding to each first joint respectively, and obtains a dynamics model corresponding to the non-wire transmission mode. For each first joint, the computer equipment determines the theoretical moment corresponding to the first joint according to the joint parameters corresponding to the first joint through a dynamic model corresponding to a non-wire transmission mode. The theoretical moment is the theoretical moment of the first joint.

For each first joint, the computer device obtains the angle value q and the speed value of the first joint

The angle value may also be used as a value of the position for the joint. Based on the speed value +. >

Determining acceleration->

Based on this, the computer device determines the theoretical moment τ of the joint aimed at by means of a dynamics model corresponding to the following non-wire drive:

wherein M (-) is a mass matrix, C (-) is a coriolis force and centripetal force matrix, and G (-) is a gravity function for deriving the gravity term.

Step S404, determining theoretical moment corresponding to each second joint through a dynamic model corresponding to the wire transmission mode according to joint parameters corresponding to each second joint.

As previously described, the second joint is driven in a wire drive. The dynamic model corresponding to the wire transmission mode is obtained by correspondingly improving the basic dynamic model by considering the principle of wire transmission and is suitable for the model of the wire transmission mode. The dynamic model corresponding to the wire drive mode takes into account the characteristics of the wire drive and the articulation of the target instrument and also takes into account the friction torque of the target instrument articulation.

Optionally, the computer device obtains joint parameters corresponding to each second joint respectively, and obtains a dynamics model corresponding to the wire transmission mode. For each second joint, the computer equipment determines the theoretical moment corresponding to the second joint according to the joint parameters corresponding to the second joint through a dynamic model corresponding to the wire transmission mode.

For each second joint, the computer device obtains the angle value q and the speed value of the second joint

For the second joint, the angle value may also be used as the value of the position. Based on the speed value +.>

Determining acceleration->

As shown in fig. 5, the second joint is a joint corresponding to the wire wheel M.

Based on this, the computer device determines the theoretical moment τ of the joint in question by means of the following dynamics model corresponding to the wire drive _M ：

Wherein τ _m Is the output torque of the motor; r is (r) _m Is the radius of the wire wheel, f is the tension on the guide wire; a is that _m Is a motor inertia matrix; q _m Is the motor angle; c (C) _m Is the motor coriolis and centripetal force matrix; f (F) _c Is a coulomb friction parameter; f (F) _s Is the maximum static friction; v _s Is the Stribeck velocity; f (F) _v Is a viscous friction parameter. Sign () is a symbol parameter.

Step S406, current values corresponding to the joints are obtained. And/or acquiring sensing data acquired by corresponding sensors of all joints.

Wherein, the joints involved in step S406 include a first joint and a second joint. The sensor refers to a stress sensor, i.e. the sensor is used to acquire stress. The sensor may be a miniature FBG (Fiber Bragg Grating ) sensor, for example. The sensed data includes an amount of change in the reflected center wavelength of the grating in the sensor.

Optionally, the computer device obtains current values corresponding to each joint of the target instrument in the current period, that is, obtains current values corresponding to each first joint and current values corresponding to each second joint.

For each joint, the on-motor data acquisition device that directly drives the joint in question acquires the current value of the joint in question, for example. The computer equipment directly acquires the current value sent by the data acquisition equipment corresponding to the aimed joint.

As mentioned above, the measured data may also include a current value due to errors in the measured data collected by the data collection device. Therefore, to ensure the accuracy of the detection, correction of the measurement data is required.

Based on this, for each joint, the data acquisition device on the motor of the joint in question is directly determined to acquire measurement data of the joint in question, and the computer device acquires the measurement data of the joint in question and performs data correction on the measurement data to obtain a current value of the joint in question, for example. Wherein the measurement data comprise joint parameters to be corrected.

Optionally, the computer device acquires sensing data corresponding to each joint, that is, acquires sensing data corresponding to each first joint and sensing data corresponding to each second joint.

Step S408, determining the actual moment corresponding to each joint according to the mapping relation between the current and the moment and the current value corresponding to each joint.

Optionally, the computer device obtains a mapping relation between the current and the moment, and for each first joint, the computer device determines an actual moment corresponding to the first joint according to the mapping relation between the current and the moment and the current value corresponding to the first joint. The actual moment is the actual moment. For each second joint, the computer equipment determines the actual moment corresponding to the second joint according to the mapping relation between the current and the moment and the current value corresponding to the second joint.

The mapping relation between the current and the moment is piecewise linear relation, namely the moment and the current are in linear relation. FIG. 6 is a schematic diagram of the current versus torque mapping in one embodiment.

Step S410, or for each first joint, acquiring first sensing data acquired by a sensor corresponding to the first joint, determining an actual moment of the first joint according to the first sensing data, acquiring current values corresponding to each second joint, and determining an actual moment corresponding to each second joint according to a mapping relation between the current and the moment.

In step S412, for each joint, the difference between the theoretical moment and the actual moment corresponding to the joint is used as the sub-external moment corresponding to the joint.

Optionally, for each first joint, the computer device regards a difference between the theoretical moment and the actual moment of the first joint being addressed as the corresponding sub-external moment of the first joint being addressed. For each second joint, the computer device uses the difference between the theoretical moment and the actual moment of the second joint as the corresponding sub-external moment of the second joint.

For example, for any one joint i, it may be a first joint or a second joint. The computer equipment determines the theoretical moment tau corresponding to the joint _expi And theoretical moment tau _fdbi Then, the corresponding sub-external moment tau of the joint _exti Is determined by the following formula:

τ _ext i＝τ _exp i-τfdbi

step S414, fusing the sub external moments corresponding to the joints respectively to obtain the external moment of the target instrument in the current period.

Optionally, the computer equipment combines the sub-external moment respectively corresponding to each first joint and the sub-external moment respectively corresponding to each second joint to obtain the external moment of the target instrument in the current period.

Based on the above steps S402, S404, S406, S408, S414, a more detailed embodiment of determining the contact force is provided. In a more specific embodiment, as shown in fig. 7, a flow chart of determining the operating force in one embodiment is shown.

Specifically, for each joint, the computer device determines the angle value and the velocity value of the joint in question from the angle value and the velocity value fed back by the motor corresponding to the joint in question. For each first joint, the computer equipment determines the theoretical moment corresponding to the first joint according to the angle value and the speed value corresponding to the first joint through a dynamics model corresponding to a non-wire transmission mode. For each second joint, the computer equipment determines the theoretical moment corresponding to the second joint according to the angle value and the speed value corresponding to the second joint through a dynamics model corresponding to the wire transmission mode.

For each joint, the computer device determines the current value of the joint in question from the current value fed back by the motor corresponding to the joint in question. For each first joint, the computer equipment determines the actual moment corresponding to the first joint according to the mapping relation between the current and the moment and the current value corresponding to the first joint. For each second joint, the computer equipment determines the actual moment corresponding to the second joint according to the mapping relation between the current and the moment and the current value corresponding to the second joint. For each first joint, the computer device takes the difference between the theoretical moment and the actual moment of the first joint as the corresponding sub-external moment of the first joint. For each second joint, the computer device uses the difference between the theoretical moment and the actual moment of the second joint as the corresponding sub-external moment of the second joint. The computer equipment combines the sub-external moment respectively corresponding to each first joint and the sub-external moment respectively corresponding to each second joint to obtain the external moment of the target instrument in the current period. The computer device obtains a Jacobian matrix for determining the contact force, and for an inverse of a transposed matrix of the Jacobian matrix, the computer device determines a value corresponding to the external force moment and reflecting the force magnitude based on the inverse and the external force moment.

In this embodiment, the dynamic model corresponding to the joint driving method is determined accordingly according to the driving method of the target instrument joint, and thus, the theoretical moment matching the joint driving method can be obtained. Meanwhile, the actual moment of each joint can be intuitively reflected based on the current value. Based on the above, the sub-external moment corresponding to each joint can be accurately determined according to the theoretical moment and the actual moment, so that the external moment of the target instrument with high accuracy can be obtained.

As described above, in order to reduce the cost while ensuring the accuracy of the force detection, the sensor may be installed only at the position related to the first joint to acquire the first sensing data corresponding to the first joint, thereby determining the actual moment corresponding to the first joint, and correspondingly, based on the current value corresponding to the second joint, thereby determining the actual moment corresponding to the second joint. Based on this, in some embodiments, as shown in fig. 8, a flow chart of determining the external torque in another embodiment is shown. The joints comprise a first joint close to the tail end of the target instrument and a second joint not close to the tail end of the target instrument, and the driving mode of the second joint is a wire transmission mode;

According to the joint parameters and the target detection mode corresponding to each joint, determining the external moment of the target instrument in the current period comprises the following steps:

step S802, determining theoretical moment corresponding to each first joint respectively through a dynamic model corresponding to a non-wire transmission mode according to joint parameters corresponding to each first joint respectively.

As previously mentioned, the joints of the target instrument include pitch, autorotation, yaw, i.e., the first joint is a pitch, or autorotation, or yaw joint. For the second joint which is not close to the tail end of the target instrument, the driving mode is a wire transmission mode, wherein two types of joints exist in the wire transmission mode, namely, a main joint directly driven by a motor, and a slave joint without motor driving is driven by a wire wheel corresponding to the main joint. As shown in fig. 5, a schematic diagram of a wire drive in one embodiment is shown. Wherein, the wire wheel M is directly connected with the motor, r _m Wire wheelRadius of M, A _m Is a motor inertia matrix. The wire wheel M is connected with the wire wheel 1 through a guide wire, the wire wheel 2 is connected with the wire wheel 1 through a guide wire, namely, the motor drives the wire wheel M to rotate, and the rotation of the wire wheel M drives the rotation of the wire wheel 1 and the wire wheel 2. The joints mentioned in the present application are main joints, pitch joints, autorotation joints, and yaw joints directly determined by the motors. The primary joint is the second joint.

The angle value may also be used as a value of the position for the joint. Based on the speed value +.>

Determining acceleration->

Step S804, determining theoretical moment corresponding to each second joint according to joint parameters corresponding to each second joint respectively through a dynamic model corresponding to the wire transmission mode.

Determining acceleration->

Step S806, for each first joint, acquiring first sensing data acquired by a sensor corresponding to the first joint, and determining an actual moment of the first joint according to the first sensing data.

As previously mentioned, a sensor refers to a stress sensor, i.e. the sensor is used to acquire stress. The sensor may be a miniature FBG (Fiber Bragg Grating ) sensor, for example. As shown in fig. 9, a schematic diagram of the sensor in one embodiment is shown. The center of the sensor is provided with an optical fiber, and the optical fiber is wrapped by a nickel-titanium alloy tube. The sensing principle is that the reflected center wavelength changes upon the application of a force to the grating.

In order to facilitate effective detection of each first joint, sensing data corresponding to the first joint are acquired through corresponding sensors, so that the actual moment of the first joint is determined. As shown in fig. 10, in an embodiment, two ends of the guide wire are fixed at two ends of the sensor, so that the sensor is suspended, and if the guide wire connected with the first joint is stressed, the sensor is compressed or stretched along the axial direction.

Wherein the first sensed data includes an amount of change in a reflected center wavelength of the grating in the sensor.

Optionally, for each first joint, the computer device acquires first sensing data acquired by a sensor corresponding to the first joint, and acquires the variation of the reflection center wavelength from the first sensing data. Based on the amount of change, the computer device determines a stress corresponding to the first joint for which it is intended, and based on the stress, determines an actual moment corresponding to the first joint for which it is intended.

For each first joint, the computer device determines the stress of the first joint according to the change amount of the reflection center wavelength and a first function after the computer device determines the change amount of the reflection center wavelength of the grating corresponding to the first joint, wherein the first function is a function that the change amount of the reflection center wavelength changes along with the change of the stress. The computer device determines a tension of the guidewire at which the first joint is directed based on the stress. The computer equipment determines the actual moment corresponding to the first joint according to the tension of the guide wire where the first joint is positioned and the second function. Wherein the second function is a function of the moment as a function of the tension.

For example, the expression of the first function is as follows:

Δλ _B ＝K _σ *σ _z

wherein Deltalambda _B Is the variation of the reflection center wavelength of the grating; k (K) _σ Representing the stress coefficient, which is a constant; sigma (sigma) _z Is the axial stress of the sensor. Illustratively, the sensor has an outer diameter of 2mm and a length of 7mm, based on which it is possible to facilitate an abnormality repair operation of the target instrument in a narrow space. Wherein the amount of change in the reflected center wavelength is positively correlated with stress.

The expression of the second function is as follows:

τ _joint ＝J*f*r

wherein τ _joint The actual moment corresponding to the first joint. J represents the motor and joint moment mappingAnd (5) a matrix of rays. f is the pulling force, and r is the arm of force from the pulling force to the rotating shaft.

Step S808, obtaining current values corresponding to the second joints respectively, and determining actual moments corresponding to the second joints respectively according to the mapping relation between the current and the moment.

Optionally, the computer device obtains current values corresponding to the second joints in the current period respectively, and determines actual moments corresponding to the second joints respectively according to the mapping relation between the current and the moment. Wherein the actual moment is the actual moment.

For each second joint, the on-motor data acquisition device that directly drives the joint in question acquires the current value of the second joint in question. The computer equipment directly acquires the current value sent by the data acquisition equipment corresponding to the second joint.

Based on this, for each second joint, the data acquisition device on the motor of the second joint in question is directly determined to acquire the measurement data of the second joint in question, and the computer device acquires the measurement data of the second joint in question and performs data correction on the measurement data to obtain the current value of the second joint in question. Wherein the measurement data comprise joint parameters to be corrected.

After obtaining the current values corresponding to the second joints respectively, the computer equipment determines the actual moment corresponding to the second joints respectively according to the current values corresponding to the second joints respectively and the mapping relation between the current and the moment.

In step S810, for each first joint, a difference between the theoretical moment and the actual moment corresponding to the first joint is used as the sub-external moment of the first joint.

Optionally, for each first joint, the computer device calculates a difference between the theoretical moment and the actual moment corresponding to the first joint, and uses the difference between the theoretical moment and the actual moment corresponding to the first joint as the sub-external moment of the first joint.

Step S812, regarding each second joint, taking the difference between the theoretical moment and the actual moment corresponding to the second joint as the sub-external moment of the second joint.

Optionally, for each second joint, the computer device calculates a difference between the theoretical moment and the actual moment corresponding to the second joint, and uses the difference between the theoretical moment and the actual moment corresponding to the second joint as the sub-external moment of the second joint.

Step S814, fusing the sub-external moment corresponding to each first joint and the sub-external moment corresponding to each second joint to obtain the external moment of the target instrument in the current period.

Based on the above steps S802 to S814, a more detailed embodiment of determining the contact force is provided. In a more specific embodiment, as shown in fig. 11, a flow chart of determining the operating force in another embodiment is shown.

For each first joint, the computer equipment acquires first sensing data acquired by a sensor corresponding to the first joint, and acquires the variation of the reflection center wavelength from the first sensing data. Based on the amount of change, the computer device determines a stress corresponding to the first joint for which it is intended, and based on the stress, determines an actual moment corresponding to the first joint for which it is intended. For each second joint, the computer device determines the current value of the joint in question from the current value fed back by the motor corresponding to the joint in question. The computer equipment obtains the current values corresponding to the second joints in the current period respectively, and determines the actual moments corresponding to the second joints respectively according to the mapping relation between the current and the moment. For each first joint, taking the difference value of the theoretical moment and the actual moment corresponding to the first joint as the sub-external moment of the first joint. And taking the difference value of the theoretical moment and the actual moment corresponding to each second joint as the sub-external moment of the second joint. And fusing the sub-external moment respectively corresponding to each first joint and the sub-external moment respectively corresponding to each second joint to obtain the external moment of the target instrument in the current period. The computer device obtains a Jacobian matrix for determining the contact force, and for an inverse of a transposed matrix of the Jacobian matrix, the computer device determines a value corresponding to the external force moment and reflecting the force magnitude based on the inverse and the external force moment.

In this embodiment, the dynamic model corresponding to the joint driving method is determined accordingly according to the driving method of the target instrument joint, and thus, the theoretical moment matching the joint driving method can be obtained. Meanwhile, the tension applied to the guide wire is monitored in real time based on the sensor deployed on the first joint, namely, after the tail end of the target instrument is subjected to the contact force, the guide wire controlling the movement of the first joint in the target instrument is subjected to the tension, and then the sensor can acquire sensing data. Based on this, the actual external moment of each first joint can be determined quickly and accurately. Based on the current value, the actual moment of each current second joint can be intuitively reflected. Based on the above, the sub-external moment corresponding to each joint can be accurately determined according to the theoretical moment and the actual moment, so that the external moment of the target instrument with high accuracy can be obtained.

In some embodiments, determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode respectively corresponding to the joints includes: and obtaining the current values corresponding to the joints respectively, and determining the actual moments corresponding to the joints respectively according to the mapping relation between the current and the moment. And determining the external torque of the target instrument in the current period through the impulse model according to the joint parameters corresponding to the joints and the actual torque corresponding to the joints.

Optionally, the computer device obtains current values corresponding to the joints respectively, and determines actual moments corresponding to the joints respectively according to the mapping relation between the current and the moment. The computer equipment acquires an impulse model constructed based on joint impulses, and determines the external moment of the target instrument in the current period through the impulse model according to the joint parameters corresponding to each joint and the actual moment corresponding to each joint.

The joints involved in this embodiment may be the first joint or the second joint, and are not distinguished. The impulse model is a model constructed based on the principle of an external force observer, i.e., for calculating moment from impulse.

For example, after determining the respective actual moment for each joint, the computer device determines, for each joint, the impulse corresponding to the joint in question, from the joint parameters corresponding to the joint in question. The computer equipment determines the external torque of the target instrument in the current period through an impulse model according to the impulse corresponding to the aimed joint and the actual torque corresponding to the aimed joint.

For example, for joint i, the angle q and velocity corresponding to joint i are determined

Then, the impulse p corresponding to the joint i is calculated by the following formula:

in the embodiment, the actual moment of each joint can be intuitively reflected based on the current value. Meanwhile, according to the parameters corresponding to the joints and the actual moment corresponding to the joints, the impulse model is directly utilized, and the external moment of the target instrument in the current period can be obtained.

In some embodiments, determining, according to the joint parameters respectively corresponding to the joints and the actual moments respectively corresponding to the joints, and through the impulse model, the external moment of the target instrument in the current period includes: determining impulse values corresponding to the joints respectively according to joint parameters corresponding to the joints respectively; at least one preamble period before the current period is determined, external force moment corresponding to each preamble period is determined, and external force moment of a target instrument in the current period is determined through an impulse model according to the external force moment corresponding to each preamble period, the actual moment corresponding to each joint in the current period and the impulse value corresponding to each joint.

Optionally, the computer device obtains joint parameters corresponding to each joint respectively, and for each joint parameter, the computer device determines impulse corresponding to the targeted joint according to the joint parameter corresponding to the targeted joint. The computer device determines a preamble period or a plurality of preamble periods preceding the current period. The computer device determines an external torque corresponding to each preamble period. The computer equipment determines the external torque of the target instrument in the current period through an impulse model according to the external torque respectively corresponding to the preamble period, the actual torque respectively corresponding to each joint in the current period and the impulse value respectively corresponding to each joint.

Illustratively, after determining the respective impulses for each joint, the computer device determines all preamble periods preceding the current period and determines the respective external moments for each preamble period. The computer equipment obtains the actual moment corresponding to each preamble period respectively, and the computer equipment carries out integral calculation through an impulse model based on the external moment and the actual moment corresponding to the preamble period respectively and the joint parameters corresponding to the preamble period respectively to determine an integral value. The computer equipment obtains the actual moment corresponding to the current period, and takes the preamble period adjacent to the current period as the last period. The computer equipment determines the current value through impulse model calculation based on the actual moment corresponding to the current period, the joint parameter corresponding to the current period and the external moment corresponding to the previous period. The computer equipment fuses impulse values corresponding to the joints respectively to obtain impulse vectors, subtracts the integrated value from the impulse vectors in the current period by subtracting the current value from the integrated value through an impulse model, determines a difference value, and determines the external moment of the target instrument in the current period based on the difference value.

For example, the external torque of the target instrument in the current cycle is calculated by the following formula:

Wherein τ _ext For the moment inside and outside the current period, K is a constant, and p is a impulse value. T1 is the current period, T1-1 is the last period,

for the actual moment corresponding to the preamble period t, C _t Coriolis force and centripetal force matrix for preamble period t +.>

For the speed value corresponding to the preamble period t, G _t And calculating the gravity item corresponding to the preamble period t according to the joint parameter corresponding to the preamble period t.

Is the external moment corresponding to the preamble period t.

For the actual moment corresponding to the current period T1, C _T1 Coriolis force and centripetal force matrix corresponding to current period +.>

G is the speed value corresponding to the current period _T1 And calculating the gravity item corresponding to the current period according to the joint parameter corresponding to the current period.

Is the external moment corresponding to the previous period.

Further, a more detailed embodiment of determining contact force is provided. In a more specific embodiment, as shown in fig. 12, a flow chart of determining the operating force in another embodiment is shown.

Specifically, for each joint, the computer device determines the angle value and the velocity value of the joint in question from the angle value and the velocity value fed back by the motor corresponding to the joint in question. For each joint, the computer device determines the current value of the joint in question from the current value fed back by the motor corresponding to the joint in question. The computer equipment determines the actual moment corresponding to the aimed joint according to the mapping relation between the current and the moment and the current value corresponding to the aimed joint. The computer equipment determines impulse values corresponding to the joints respectively according to the joint parameters corresponding to the joints respectively; at least one preamble period before the current period is determined, external force moment corresponding to each preamble period is determined, and external force moment of a target instrument in the current period is determined through an impulse model according to the external force moment corresponding to each preamble period, the actual moment corresponding to each joint in the current period and the impulse value corresponding to each joint. The computer device obtains a Jacobian matrix for determining the contact force, and for an inverse of a transposed matrix of the Jacobian matrix, the computer device determines a value corresponding to the external force moment and reflecting the force magnitude based on the inverse and the external force moment.

In the embodiment, the actual moment of each joint can be intuitively reflected based on the current value. Based on the above, in the process of determining the external torque in the current period, the external torque of the target instrument in the current period is accurately estimated by using the impulse model and combining the actual torque and the external torque corresponding to the preamble period and the actual torque of the current period.

As described above, in order to ensure accuracy of the subsequent contact force, the measured data obtained in the current period may be calibrated in advance before the contact force in the current period is determined based on the measured data obtained in the current period, so that the effectiveness and reliability of the contact force may be greatly improved.

To this end, in some embodiments, acquiring joint parameters corresponding to respective joints of a target instrument during a current cycle includes: and acquiring a value of the contact force in the previous period, taking a period adjacent to and before the previous period as a target preamble period, and acquiring target parameters respectively corresponding to all joints of the target instrument in the target preamble period, wherein the target parameters comprise joint parameters. Determining a measurement error according to the corresponding target parameters of each joint of the target instrument in the target preamble period and the contact force value in the previous period; and acquiring measurement data which are fed back by a motor in the target instrument and respectively correspond to each joint in the current period. And correcting the measurement data according to the measurement error to obtain target parameters corresponding to all the joints of the target instrument in the current period, and extracting joint parameters from the target parameters corresponding to all the joints of the target instrument in the current period.

The period between the current period and the target preamble period is the previous period, and the target parameters corresponding to the joints in the target preamble period respectively refer to parameters after data correction, and the target parameters comprise joint parameters and current values corresponding to the joints in the target preamble period respectively.

From the foregoing, it can be seen that the contact force in the current period is determined based on the joint parameters in the current period, and how the contact force in the previous period is determined based on the target parameters corresponding to the respective joints in the target preamble period can be briefly described by linearizing the following state equation:

x _t the target state corresponding to the target parameter for the previous period t, i.e., the value representing the corrected target parameter. X is x _t-1 For the target state, z corresponding to the target parameter in the target preamble period t-1 _t The noise term delta is the value of the t contact force of the previous period _t 、∈ _t All are considered to be gaussian distributions of zero mean. F, representing the mapping relation between the target parameter after correction in the target preamble period and the target parameter in the previous period. And H represents the mapping relation between the corrected target parameter in the previous period and the contact force in the previous period. The above-described process is a correction process based on the kalman filter principle.

Optionally, the computer device obtains a value of the contact force in the previous cycle, takes a cycle adjacent to and before the previous cycle as a target preamble cycle, and obtains target parameters corresponding to each joint of the target instrument in the target preamble cycle. The computer equipment determines the measurement error according to the target parameters respectively corresponding to the joints of the target instrument in the target preamble period and the contact force value in the previous period. The method comprises the steps that computer equipment obtains measurement data which are fed back by a motor in a target instrument and correspond to all joints in a current period respectively; and correcting the measurement data according to the measurement error to obtain target parameters corresponding to all the joints of the target instrument in the current period, and extracting joint parameters from the target parameters corresponding to all the joints of the target instrument in the current period. When the current period is the first period, the values of the target parameters respectively corresponding to the joints of the target instrument in the target preamble period and the contact force in the previous period are respectively the preset parameters and the preset contact force values.

The measured data are data which are not corrected and correspond to the joints in the current period respectively, and the measured data are corrected to obtain target parameters. The measurement data includes unmodified joint parameters and current values.

Illustratively, as shown in FIG. 13, a flow diagram of the correction in one embodiment is shown. The computer equipment determines a target test mode from a plurality of preset first test modes, second test modes and third test modes, and after determining the value of the contact force in the previous period based on the target test mode, the computer equipment performs Kalman filtering calculation, namely, determines the measurement error according to the target parameters respectively corresponding to each joint of the target instrument in the target preamble period and the value of the contact force in the previous period. Based on the measurement error, the computer equipment performs state update (i.e. correction) on the measurement data corresponding to each joint in the current period to obtain the target parameters corresponding to each joint of the target instrument in the current period, and extracts the joint parameters from the target parameters corresponding to each joint of the target instrument in the current period to determine the contact force in the current period, and outputs the contact force in the current period. If the current period is the first period, the computer equipment acquires preset parameters corresponding to all joints respectively as target parameters corresponding to all joints respectively in a target preamble period and acquires a preset contact force value as a contact force value in the previous period before determining the measurement error. And then the calculation of the next cycle is performed.

Specifically, the target parameters corresponding to the joint parameters in the target preamble period t-1 are fused to obtain the estimated value of the target preamble period t-1

An estimated value of the target preamble period t-1 is +.>

Processing to determine expected value of the last period t corresponding to the estimated value>

Updating the expected parameters of the covariance matrix corresponding to the previous period t by the following formula II

P _t And the parameter matrix corresponding to the previous period t. Then determining the Kalman coefficient matrix K corresponding to the previous period through the following formula III _t ：

Q in the formula II and R in the formula III are two parameters corresponding to the covariance matrix. Then determining the estimated value P of the covariance matrix of the current period according to the following formula IV _t+1 (measurement error calculation for determining the next cycle):

i in the above formula IV is an identity matrix. Finally, determining the estimation error corresponding to the previous period based on the following formula five

Z in the above formula five _t The estimated error corresponding to the previous period is the value of the contact force corresponding to the previous period t

I.e. the measurement error of the current period.

In this embodiment, all values included in the measurement data are corrected synchronously in a parallel manner. Of course, the above procedure may be adopted to sequentially correct each value in the measurement data in a serial manner.

In this embodiment, the measurement error for correction processing in the current period can be determined by the value of the contact force in the previous period and the target parameters corresponding to each joint in the target preamble period before the previous period, and based on this, the data calibration can be performed on the measurement data obtained in the current period in advance, so that the effectiveness and reliability of the contact force can be greatly improved.

As the number of abnormal repair operations increases, the target instrument may be damaged, and particularly, when a preset threshold number of uses is reached, the target instrument may come loose or even break, and at this time, the target instrument may not reach the desired position. FIG. 14 is a schematic diagram of a position of a distal end of a target instrument according to an embodiment, where an operator may make the target instrument reach a desired position, i.e. an actual position of the distal end of the target instrument is the desired position, through master-slave control when the number of uses of the target instrument does not reach a threshold number of uses; when the number of uses of the target instrument reaches the threshold number of uses, the operator needs to make the target instrument reach the desired position by master-slave control, but in reality the actual position reached by the end of the target instrument does not coincide with the desired position.

Based on this, in some embodiments, the method further comprises: and under the condition that the using times of the target instrument are the using times threshold value, determining the value of the force to be compensated of the tail end of the target instrument according to the value of the contact force corresponding to the current period. And determining the compensation moment corresponding to the force to be compensated in the current period through the mapping relation between the translational degree of freedom and the pose degree of freedom according to the value of the force to be compensated. And determining the theoretical moment corresponding to each joint in the current period. And determining a target moment according to the theoretical moment and the compensation moment respectively corresponding to each joint in the current period, wherein the target moment is used for enabling the target instrument to reach the expected position.

Since each target instrument has a service life, the service life is represented by a use number threshold, for example, the use number threshold may represent the last use number, the last two use numbers, the last three use numbers, or the like, which is not particularly limited. For example, the service life is 5 times, that is, the threshold of the use times is 5, and the last use time is 5 th.

Optionally, in the case that the number of uses of the target instrument is a threshold number of uses, the computer device determines, according to the value of the contact force corresponding to the current cycle, the value of the force to be compensated at the end of the target device by means of a preset gain coefficient. And the computer equipment determines the compensation moment corresponding to the force to be compensated in the current period through the mapping relation between the translational degree of freedom and the pose degree of freedom according to the value of the force to be compensated. The computer equipment determines the theoretical moment corresponding to each joint in the current period. The computer equipment fuses the theoretical moment and the compensation moment corresponding to each joint in the current period to determine the target moment.

Illustratively, as shown in fig. 15, a flow chart of the step of determining the target torque in one embodiment is shown. In fig. 15, the theoretical torque is determined in the first detection mode or the second detection mode. Of course, the theoretical moment may be determined by a third detection method, that is, after the target moment in the current period is determined, the theoretical moment in the current period may be obtained by fusing the target moment and the actual moment in the current period.

And the computer equipment determines the theoretical moment corresponding to each joint according to the joint parameters corresponding to each joint respectively and the corresponding dynamics model, fuses the theoretical moment corresponding to each joint respectively and determines the corresponding theoretical moment vector. When the joint is a first joint, the corresponding dynamic model is a dynamic model corresponding to a non-wire transmission mode; when the joint is a second joint, the corresponding dynamic model is a dynamic model corresponding to the wire transmission mode. The computer equipment takes the product of the contact force value and the gain coefficient as the force value to be compensated, determines the compensation moment corresponding to the force to be compensated in the current period through the mapping relation between the translational degree of freedom and the pose degree of freedom, and fuses the compensation moment and the theoretical moment to obtain the target moment. The target instrument is controlled to a desired position by the joint controller based on the target torque.

In the process of determining the theoretical moment through the corresponding dynamics model, namely, for each joint, determining an acceleration value based on the velocity value of the joint, and inputting the angle value, the velocity value and the acceleration value of the joint into the dynamics model corresponding to the joint to respectively obtain the gravity moment tau _g Moment of inertia τ _I And friction moment tau _f Wherein:

τ _g ＝G(q)

based on this, the theoretical moment τ determination process can be simplified to a gravity moment τ _g Moment of inertia τ _I And friction moment tau _f And (3) superposing, namely:

τ＝τ _I +τ _g +τ _f

wherein for the compensation moment τ _ctrl In the determination of (2), the force f to be compensated is determined by the following formula _ctrl ：

f _ctrl ＝-sign(f _ext )Kf _ext

Wherein sign (-) is a sign function, f _ext K is a gain factor, which is the value of the contact force. Then, the mapping relationship between the translational degrees of freedom and the pose degrees of freedom can be regarded as an acrylic matrix J containing 6 degrees of freedom information, namely:

τ _ctrl ＝J ^T f _ctrl

in this embodiment, the compensation moment is determined according to the mapping relation between the translational degrees of freedom and the pose degrees of freedom, so that the moment capable of representing the translational motion and the pose motion is obtained, and based on the moment, the 6 degrees of freedom can be directly controlled according to the target moment determined by the compensation moment and the theoretical moment. In this way, when the number of uses of the target instrument reaches the threshold number of uses, the problems of inaccurate control and reduced controllability due to damage to the target instrument can be improved, thereby ensuring the accuracy of the abnormality repair operation.

In some embodiments, the method further comprises: and under the condition that the using times of the target instrument are the using times threshold value, the pose moment generated when each first joint moves under the driving of the motor is obtained. And acquiring theoretical moments corresponding to the joints in the current period respectively, and determining the translation moment based on the value of the contact force corresponding to the current period and the theoretical moment corresponding to the joints in the current period respectively. And determining a target moment according to the pose moment and the translation moment, wherein the target moment is used for enabling the target instrument to reach the expected position.

Optionally, under the condition that the use times of the target instrument are the use times threshold, the computer equipment obtains pose moments generated when each first joint moves under the drive of the motor, and obtains theoretical moments corresponding to each joint in the current period. The computer device determines a translational torque based on the value of the contact force corresponding to the current period and the theoretical torque corresponding to each joint in the current period. The computer equipment fuses the pose moment and the translation moment to determine the target moment.

It should be noted that, as shown in fig. 16, a schematic diagram of the target apparatus in one embodiment is shown, where the rotation control is performed by the motors of the pitch joint, the rotation joint, and the yaw joint, that is, the 3 degrees of freedom of pose are controlled. The force control is performed by the contact force in the current period, namely, whether the current contact force causes the clamping force to be excessive in the clamping state of the target instrument is determined, and the three translation directions of x, y and z are involved in the control of the clamping force, namely, the 3 translation degrees of freedom are controlled. Based on the method, the translation moment corresponding to the translation freedom degree and the pose moment corresponding to the pose freedom degree are fused, the control of 6 degrees of freedom of the tail end of the target instrument can be realized, in addition, the condition that the target instrument reaches the expected position is ensured, the clamping force of the target instrument can be ensured not to be too large, and the damage to the target instrument is avoided.

Illustratively, as shown in fig. 17, a flow chart of the step of determining the target torque in another embodiment is shown. If the number of uses of the target instrument is the threshold number of uses, and if the target instrument does not reach the desired position or the desired contact force (at this time, the desired contact force does not damage the target instrument and the repair site) in the previous cycle, the computer device acquires the pose moment generated when each first joint moves under the drive of the motor, so as to perform rotation control. Meanwhile, the computer equipment determines translation moment based on the value of the contact force corresponding to the current period and the theoretical moment corresponding to each joint in the current period so as to control the force in the translation direction. The computer equipment fuses the pose moment and the translation moment, determines the target moment, controls the operation of the target instrument through the target moment, acquires the actual position of the target instrument after the operation of the target instrument is completed, and continuously performs rotation control and force control in the next period if the actual position is not the desired position. Or after the operation of the target instrument is completed, judging whether the environment of the target instrument after the operation is completed, namely the clamping force of the target instrument is overlarge, and if so, damaging the target instrument. If the clamping force is too large, the rotation control and the force control are continued in the next period.

In this embodiment, when the number of uses of the target instrument is the threshold number of uses, by determining the translational moment corresponding to the translational degree of freedom and the pose moment corresponding to the pose degree of freedom, respectively, it is possible to ensure that the target instrument reaches the expected position, and to observe in real time that the clamping force of the target instrument is not excessive. Thus, while avoiding damage to the target instrument, it is also ensured that the target instrument accurately reaches the intended location.

In one embodiment, determining the translational torque based on the value of the contact force corresponding to the current period and the theoretical torque corresponding to each joint in the current period, includes: and determining the value of the force to be compensated of the tail end of the target instrument according to the value of the contact force corresponding to the current period. And determining the compensation moment corresponding to the force to be compensated in the current period according to the value of the force to be compensated through the mapping relation of the translational degrees of freedom. And determining the translation moment according to the compensation moment and the theoretical moment corresponding to each joint in the current period.

Optionally, the computer device determines the value of the force to be compensated at the end of the target according to the value of the contact force corresponding to the current period through a preset gain coefficient. And the computer equipment determines the compensation moment corresponding to the force to be compensated in the current period through the mapping relation of the translational degrees of freedom according to the value of the force to be compensated. The computer equipment fuses the theoretical moment and the compensation moment corresponding to each joint in the current period to determine the translation moment.

Illustratively, the computer device calculates a product of a value of the contact force corresponding to the current period and a preset gain factor, and determines a value of the force to be compensated for at the target tip based on the product. And the computer equipment determines the compensation moment corresponding to the force to be compensated in the current period through the mapping relation of the translational degrees of freedom according to the value of the force to be compensated. The computer equipment fuses the theoretical moment and the compensation moment corresponding to each joint in the current period to determine the translation moment.

Specifically, the compensation force f _ctrl The determination can be made by the following formula:

f _ctrl ＝-sign(f _ext )Kf _ext

wherein sign (-) is a sign function, f _ext K is a gain factor, which is the value of the contact force.

In this embodiment, when the number of uses of the target instrument is the threshold number of uses, the compensation moment in the current period can be accurately determined according to the mapping relation of the translational degrees of freedom, so that the translational moment in the current period can be determined, which is beneficial to effectively detecting whether the clamping force of the target instrument is excessive or not.

In some embodiments, the method further comprises: and sending an alarm signal to the display device when the value of the contact force is greater than or equal to the threshold value.

The display device can be a display screen on the operation workbench or a display screen on the image trolley. Fig. 18 is a schematic view showing the structure of the operation table in one embodiment. The operation table includes: an adjustment part 110, a manipulation arm 120, a trolley part 130, an image part 140 (i.e., a display screen). The two control arms 120 detect hand movement information of an operator through control handles at the ends thereof as movement control input; the dolly part 130 is a base bracket for mounting other components, and the control dolly has movable casters that can be moved or fixed as required; the trolley part 130 is provided with a foot switch for detecting a switching value control signal sent by an operator; the adjusting component 110 can electrically adjust the position of the manipulator arm, the image component, the operator armrest, etc., i.e., the man-machine parameter adjusting function. The image unit 140 may provide the operator with a stereoscopic image detected from the image system, and provide the operator with reliable image information for performing an abnormality repair operation. In the abnormality repairing operation, an operator sitting on the operation table is located outside the sterilization area, and the operator controls the target instrument and the laparoscope by operating the control handle at the distal end of the manipulation arm. The operator observes the intracavity picture of returning through the image part, and both hands action control repair work platform truck arm and target apparatus motion accomplish various operations to carry out unusual repair operation, the operation of operating personnel accessible foot switch control part action simultaneously, accomplish relevant operation input such as electric cutting, electric coagulation through foot switch.

Optionally, the computer device obtains a value of the contact force and compares the value of the contact force with a threshold value. And sending an alarm signal to a display screen on the operation workbench and a display screen on the image trolley under the condition that the value of the contact force is greater than or equal to a threshold value.

It should be noted that, the threshold value is used for warning, and once the value of the contact force is not smaller than the threshold value, it is indicated that the contact between the target instrument and the repair site may cause damage to the repair site and the end of the target machine, and corresponding personnel intervention is required. At the same time, the display screen of the operation table also displays the life of the target instrument and the contact force value, as shown in fig. 19, which is a schematic diagram of the display device in one embodiment. The display device shows that the target instrument is a bipolar duckbill pliers, the service life is 5 times, and the contact force value in the current period is 1.2N. Based on the numbers displayed on the display device, the contact force of the tail end can be adjusted, the target instrument is protected from excessive external force, and the reliability of the target instrument is improved. In addition, as shown in fig. 18, once the value of the contact force is not less than the threshold value, the operation table also sends a command to control the target instrument not to continue to move in the direction in which the contact force increases, avoiding the target instrument from being damaged due to excessive external force.

The computer device, for example, performs a contact force detection, i.e. compares the contact force with a reminder threshold, and if the contact force does not exceed the reminder threshold (i.e. the contact force is less than the reminder threshold), then a determination of the next cycle of contact force is made. If the contact force exceeds the prompt threshold (i.e., the contact force is not less than the prompt threshold), the computer device feeds back the contact force to the main end (the operation workbench) so that the operator can feel the corresponding force and prompt the operator to pay attention. If the contact force exceeds the threshold (i.e., the contact force is not less than the threshold, the threshold is greater than the alert threshold), the computer device sends an alert signal to a display screen on the operator console, and to a display screen on the image trolley. If the contact force does not exceed the threshold (i.e., the contact force is less than the threshold), the computer device feeds back the contact force to the main end (the operating table) so that the operator can feel the corresponding force, prompting the operator to continue paying attention.

Wherein the prompt threshold is for prompting, but not alerting. The hint threshold is less than a threshold, for example, the hint threshold is 1 and the threshold is 2. When the contact force exceeds the prompt threshold, or the contact force exceeds the prompt threshold and is smaller than the threshold, the computer equipment determines the feedback force based on the master-slave control and the contact force and sends a feedback moment corresponding to the feedback force to the master end (the operation workbench), so that an operator can feel the corresponding force, the operation difficulty is reduced, and the safety of the abnormal repair operation is improved. For example:

f _master ＝scale*f _slave

T _master ＝(J0 ^T )*f _master

In the above formula, f _slave For the value of the contact force in the current period, scale is the mapping ratio, f _master Is a feedback force, J0 is a machine corresponding to the operation tableJacobian matrix of arms, τ _master Is the expected feedback moment of the mechanical arm corresponding to the operation workbench.

In this embodiment, when the value of the contact force is greater than or equal to the threshold value, an alarm signal is sent to the display device, so that both the target instrument and the repair site can be effectively and safely protected.

In one embodiment, a more detailed embodiment is provided for the purpose of facilitating a clearer understanding of the technical solution of the present application. The computer equipment comprises a force sensing unit, a sensing data acquisition unit, a sensing data processing unit, an instrument control unit, a contact force detection unit and an instrument operation force safety protection unit. The force sensing unit comprises a sensor, a motor feedback current collector, a motor and a joint position encoder. And the sensing data acquisition unit is used for acquiring real-time sensing data of the sensor. And the sensing data processing unit is used for processing the sensing data acquired by the sensor acquisition unit and converting the data into the stress of the guide wire. And the instrument control unit is used for sending a motion instruction to the slave-end instrument according to the master hand motion information so as to realize master-slave following. And the contact force detection unit is used for calculating the value of the instrument contact force according to the target detection mode. And the instrument operation force safety protection unit is used for enabling an operator at the main end to feel the stress condition of the tail end of the target instrument through master-slave force feedback, and giving an alarm through an image when the contact force is overlarge so as to prevent the damage of the target instrument or the repair part due to the overlarge contact force.

Specifically, based on the operation workbench and the repair workbench, master-slave operation is performed, that is, the computer equipment sets the operation workbench as a master end, the repair workbench as a slave end, and master-slave mapping is performed, so that data interaction can be performed between the operation workbench and the repair workbench.

The contact force detection unit in the computer equipment acquires the value of the contact force in the previous period, takes the period adjacent to and before the previous period as a target preamble period, and acquires target parameters corresponding to each joint of the target instrument in the target preamble period, wherein the target parameters comprise joint parameters. And determining a measurement error according to the target parameters respectively corresponding to the joints of the target instrument in the target preamble period and the contact force value in the previous period. And acquiring measurement data which are fed back by a motor in the target instrument and respectively correspond to each joint in the current period. And correcting the measurement data according to the measurement error to obtain target parameters corresponding to all the joints of the target instrument in the current period, and extracting joint parameters from the target parameters corresponding to all the joints of the target instrument in the current period.

The joints include a first joint near the end of the target instrument and a second joint not near the end of the target instrument, the second joint being driven in a wire drive manner.

If the target detection mode is the first detection mode, a contact force detection unit in the computer equipment determines theoretical moment corresponding to each first joint respectively through a dynamic model corresponding to the non-wire transmission mode according to joint parameters corresponding to each first joint respectively. And determining the theoretical moment corresponding to each second joint respectively through a dynamic model corresponding to the wire transmission mode according to the joint parameters corresponding to each second joint respectively. And obtaining current values corresponding to the joints respectively. And determining the actual moment corresponding to each joint according to the mapping relation between the current and the moment and the current value corresponding to each joint. For each joint, taking the difference between the theoretical moment and the actual moment corresponding to the joint as the sub-external moment corresponding to the joint. And fusing the sub external moments corresponding to the joints respectively to obtain the external moment of the target instrument in the current period.

If the target detection mode is the second detection mode, a contact force detection unit in the computer equipment determines theoretical moment corresponding to each second joint respectively through a dynamic model corresponding to the wire transmission mode according to joint parameters corresponding to each second joint respectively. For each first joint, acquiring first sensing data acquired by a sensor corresponding to the first joint, and determining the actual moment of the first joint according to the first sensing data. And obtaining the current values corresponding to the second joints respectively, and determining the actual moments corresponding to the second joints respectively according to the mapping relation between the current and the moment. For each first joint, taking the difference value of the theoretical moment and the actual moment corresponding to the first joint as the sub-external moment of the first joint. And taking the difference value of the theoretical moment and the actual moment corresponding to each second joint as the sub-external moment of the second joint. And fusing the sub-external moment respectively corresponding to each first joint and the sub-external moment respectively corresponding to each second joint to obtain the external moment of the target instrument in the current period.

If the target detection mode is the second detection mode, a contact force detection unit in the computer equipment obtains current values corresponding to all joints respectively, and determines actual moments corresponding to all joints respectively according to the mapping relation between the current and the moment. And determining the impulse value corresponding to each joint according to the joint parameters corresponding to each joint. At least one preamble period before the current period is determined, external force moment corresponding to each preamble period is determined, and external force moment of a target instrument in the current period is determined through an impulse model according to the external force moment corresponding to each preamble period, the actual moment corresponding to each joint in the current period and the impulse value corresponding to each joint.

After the external torque of the target instrument is determined, a contact force detection unit in the computer device determines a value corresponding to the external torque and used for reflecting the force according to the mapping relation between the torque and the force, and takes the determined value as the value of the contact force of the tail end of the target instrument in the current period.

And under the condition that the using times of the target instrument are the using times threshold, a contact force detection unit in the computer equipment determines the value of the force to be compensated of the tail end of the target instrument according to the value of the contact force corresponding to the current period. And determining the compensation moment corresponding to the force to be compensated in the current period through the mapping relation between the translational degree of freedom and the pose degree of freedom according to the value of the force to be compensated. And determining the theoretical moment corresponding to each joint in the current period. And determining a target moment according to the theoretical moment and the compensation moment respectively corresponding to each joint in the current period, wherein the target moment is used for enabling the target instrument to reach the expected position.

Or under the condition that the using times of the target instrument are the using times threshold value, a contact force detection unit in the computer equipment obtains pose moment generated when each first joint moves under the drive of a motor; and acquiring theoretical moment corresponding to each joint in the current period, and determining the value of the force to be compensated at the tail end of the target instrument according to the value of the contact force corresponding to the current period. And determining the compensation moment corresponding to the force to be compensated in the current period according to the value of the force to be compensated through the mapping relation of the translational degrees of freedom. And determining the translation moment according to the compensation moment and the actual moment corresponding to each joint in the current period. And determining a target moment according to the pose moment and the translation moment, wherein the target moment is used for enabling the target instrument to reach the expected position.

The touch force detection unit in the computer device can judge whether the touch force exceeds a threshold value (namely, whether the touch force is larger than or equal to the threshold value, and if the touch force is larger than or equal to the threshold value, the safety protection unit in the computer device feeds back to the main terminal and sends out an alarm signal to the display device.

In the embodiment, the joint parameters corresponding to the joints of the target instrument in the current period are obtained; the joint parameters include an angle value and a velocity value of the joint. And determining a target detection mode from a plurality of preset detection modes. And determining the external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint. That is, the external moment can be directly determined by a preset target detection mode through the joint parameters corresponding to the joints in the current period, and errors caused by indirect measurement can be effectively avoided. According to the mapping relation between the moment and the force, the numerical value which corresponds to the external moment and is used for reflecting the force can be rapidly and accurately determined, and the determined numerical value is used as the value of the contact force of the tail end of the target instrument in the current period. Based on the method, through the joint parameters corresponding to the joints of the target instrument in the current period, the contact force generated by the contact between the target instrument and the repair part can be directly and accurately detected by utilizing the target detection mode, and the measurement error generated by indirect measurement is effectively avoided, so that the precision of instrument contact force detection is improved.

It should be understood that, although the steps in the flowcharts related to the embodiments described above are sequentially shown as indicated by arrows, these steps are not necessarily sequentially performed in the order indicated by the arrows. The steps are not strictly limited to the order of execution unless explicitly recited herein, and the steps may be executed in other orders. Moreover, at least some of the steps in the flowcharts described in the above embodiments may include a plurality of steps or a plurality of stages, which are not necessarily performed at the same time, but may be performed at different times, and the order of the steps or stages is not necessarily performed sequentially, but may be performed alternately or alternately with at least some of the other steps or stages.

Based on the same inventive concept, the embodiments of the present application also provide an apparatus for detecting an apparatus contact force for implementing the above-mentioned method for detecting an apparatus contact force. The implementation of the solution provided by the device is similar to that described in the above method, so the specific limitations in the embodiments of the device for detecting a contact force of one or more instruments provided below can be referred to the limitations of the method for detecting a contact force of an instrument hereinabove, and will not be repeated here.

In one embodiment, as shown in fig. 20, there is provided a device for detecting an instrument contact force, comprising: a parameter acquisition module 2002, a detection mode determination module 2004, an external torque determination module 2006, and a contact force determination module 2008, wherein:

the parameter obtaining module 2002 is configured to obtain joint parameters corresponding to each joint of the target instrument in the current period; the joint parameters include an angle value and a velocity value of the joint.

A detection mode determining module 2004, configured to determine a target detection mode from a plurality of preset detection modes;

the external moment determining module 2006 is configured to determine an external moment of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint.

The contact force determining module 2008 is configured to determine a value corresponding to the external torque and reflecting the magnitude of the force according to the mapping relationship between the torque and the force, and take the determined value as the value of the contact force of the distal end of the target instrument in the current period.

In one embodiment, the joints include a first joint near the end of the target instrument and a second joint not near the end of the target instrument, the second joint being driven in a wire drive manner; the external torque determining module 2006 is configured to determine, according to the joint parameters corresponding to each first joint, a theoretical torque corresponding to each first joint through a dynamic model corresponding to a non-wire transmission mode. And determining the theoretical moment corresponding to each second joint respectively through a dynamic model corresponding to the wire transmission mode according to the joint parameters corresponding to each second joint respectively. And acquiring current values corresponding to the joints respectively, and/or acquiring sensing data acquired by sensors corresponding to the joints. And determining the actual moment corresponding to each joint according to the mapping relation between the current and the moment and the current value corresponding to each joint. Or for each first joint, acquiring first sensing data acquired by a sensor corresponding to the first joint, determining the actual moment of the first joint according to the first sensing data, acquiring current values corresponding to each second joint respectively, and determining the actual moment corresponding to each second joint according to the mapping relation between the current and the moment; for each joint, taking the difference between the theoretical moment and the actual moment corresponding to the joint as the sub-external moment corresponding to the joint. And fusing the sub external moments corresponding to the joints respectively to obtain the external moment of the target instrument in the current period.

In one embodiment, the external torque determining module 2006 is configured to obtain current values corresponding to the joints respectively, and determine actual torques corresponding to the joints respectively according to a mapping relationship between the current and the torque. And determining the external torque of the target instrument in the current period through the impulse model according to the joint parameters corresponding to the joints and the actual torque corresponding to the joints.

In one embodiment, the external moment determining module 2006 is configured to determine the impact value corresponding to each joint according to the joint parameter corresponding to each joint. At least one preamble period before the current period is determined, external force moment corresponding to each preamble period is determined, and external force moment of a target instrument in the current period is determined through an impulse model according to the external force moment corresponding to each preamble period, the actual moment corresponding to each joint in the current period and the impulse value corresponding to each joint.

In one embodiment, the parameter obtaining module 2002 is configured to obtain a value of the contact force in a previous cycle, take a cycle adjacent to and before the previous cycle as a target preamble cycle, and obtain target parameters corresponding to each joint of the target instrument in the target preamble cycle, where the target parameters include joint parameters. And determining a measurement error according to the target parameters respectively corresponding to the joints of the target instrument in the target preamble period and the contact force value in the previous period. And acquiring measurement data which are fed back by a motor in the target instrument and respectively correspond to each joint in the current period. And correcting the measurement data according to the measurement error to obtain target parameters corresponding to all the joints of the target instrument in the current period, and extracting joint parameters from the target parameters corresponding to all the joints of the target instrument in the current period.

In one embodiment, the device further includes a target torque determination module configured to determine a value of the force to be compensated at the distal end of the target instrument based on the value of the contact force corresponding to the current cycle if the number of uses of the target instrument is the threshold number of uses. And determining the compensation moment corresponding to the force to be compensated in the current period through the mapping relation between the translational degree of freedom and the pose degree of freedom according to the value of the force to be compensated. And determining the theoretical moment corresponding to each joint in the current period. And determining a target moment according to the theoretical moment and the compensation moment respectively corresponding to each joint in the current period, wherein the target moment is used for enabling the target instrument to reach the expected position.

In one embodiment, the target moment determining module is configured to obtain, when the number of uses of the target instrument is a threshold number of uses, a pose moment generated when each first joint moves under the driving of the motor. And acquiring theoretical moments corresponding to the joints in the current period respectively, and determining the translation moment based on the value of the contact force corresponding to the current period and the theoretical moment corresponding to the joints in the current period respectively. And determining a target moment according to the pose moment and the translation moment, wherein the target moment is used for enabling the target instrument to reach the expected position.

In one embodiment, the target torque determination module is configured to determine a value of the force to be compensated for at the distal end of the target instrument based on a value of the contact force corresponding to the current cycle. And determining the compensation moment corresponding to the force to be compensated in the current period according to the value of the force to be compensated through the mapping relation of the translational degrees of freedom. And determining the translation moment according to the compensation moment and the actual moment corresponding to each joint in the current period.

In one embodiment, the apparatus further comprises a sending module for sending an alarm signal to the display device in case the value of the contact force is greater than or equal to the threshold value.

The various modules in the device for detecting the contact force of the instrument described above may be implemented in whole or in part by software, hardware, and combinations thereof. The above modules may be embedded in hardware or may be independent of a processor in the computer device, or may be stored in software in a memory in the computer device, so that the processor may call and execute operations corresponding to the above modules.

In one embodiment, a computer device is provided, which may be a server, and the internal structure of which may be as shown in fig. 21. The computer device includes a processor, a memory, an Input/Output interface (I/O) and a communication interface. The processor, the memory and the input/output interface are connected through a system bus, and the communication interface is connected to the system bus through the input/output interface. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The input/output interface of the computer device is used to exchange information between the processor and the external device. The communication interface of the computer device is used for communicating with an external terminal through a network connection. The computer program is executed by a processor to implement a method of detecting an instrument contact force.

It will be appreciated by those skilled in the art that the structure shown in fig. 21 is merely a block diagram of a portion of the structure associated with the present application and is not limiting of the computer device to which the present application applies, and that a particular computer device may include more or fewer components than shown, or may combine some of the components, or have a different arrangement of components.

In an embodiment, there is also provided a computer device comprising a memory and a processor, the memory having stored therein a computer program, the processor implementing the steps of the method embodiments described above when the computer program is executed.

In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored which, when executed by a processor, carries out the steps of the method embodiments described above.

In an embodiment, a computer program product is provided, comprising a computer program which, when executed by a processor, implements the steps of the method embodiments described above.

It should be noted that, the user information (including, but not limited to, user equipment information, user personal information, etc.) and the data (including, but not limited to, data for analysis, stored data, presented data, etc.) referred to in the present application are information and data authorized by the user or sufficiently authorized by each party, and the collection, use and processing of the related data are required to comply with the related laws and regulations and standards of the related countries and regions.

Those skilled in the art will appreciate that implementing all or part of the above described methods may be accomplished by way of a computer program stored on a non-transitory computer readable storage medium, which when executed, may comprise the steps of the embodiments of the methods described above. Any reference to memory, database, or other medium used in the various embodiments provided herein may include at least one of non-volatile and volatile memory. The nonvolatile Memory may include Read-Only Memory (ROM), magnetic tape, floppy disk, flash Memory, optical Memory, high density embedded nonvolatile Memory, resistive random access Memory (ReRAM), magnetic random access Memory (Magnetoresistive Random Access Memory, MRAM), ferroelectric Memory (Ferroelectric Random Access Memory, FRAM), phase change Memory (Phase Change Memory, PCM), graphene Memory, and the like. Volatile memory can include random access memory (Random Access Memory, RAM) or external cache memory, and the like. By way of illustration, and not limitation, RAM can be in the form of a variety of forms, such as static random access memory (Static Random Access Memory, SRAM) or dynamic random access memory (Dynamic Random Access Memory, DRAM), and the like. The databases referred to in the various embodiments provided herein may include at least one of relational databases and non-relational databases. The non-relational database may include, but is not limited to, a blockchain-based distributed database, and the like. The processors referred to in the embodiments provided herein may be general purpose processors, central processing units, graphics processors, digital signal processors, programmable logic units, quantum computing-based data processing logic units, etc., without being limited thereto.

The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.

The above examples only represent a few embodiments of the present application, which are described in more detail and are not to be construed as limiting the scope of the present application. It should be noted that it would be apparent to those skilled in the art that various modifications and improvements could be made without departing from the spirit of the present application, which would be within the scope of the present application. Accordingly, the scope of protection of the present application shall be subject to the appended claims.

Claims

1. A method of detecting an instrument contact force, the method comprising:

determining a target detection mode from a plurality of preset detection modes;

2. The method of claim 1, wherein the joints comprise a first joint proximate the distal end of the target instrument and a second joint not proximate the distal end of the target instrument, the second joint being driven in a wire drive manner;

the determining the external moment of the target instrument in the current period according to the joint parameters respectively corresponding to the joints and the target detection mode comprises the following steps:

according to joint parameters corresponding to the first joints respectively, determining theoretical moment corresponding to the first joints respectively through a dynamics model corresponding to a non-wire transmission mode;

according to joint parameters corresponding to the second joints respectively, determining theoretical moment corresponding to the second joints respectively through a dynamic model corresponding to the wire transmission mode;

acquiring current values corresponding to the joints respectively, and/or acquiring sensing data acquired by sensors corresponding to the joints;

determining the actual moment corresponding to each joint according to the mapping relation between the current and the moment and the current value corresponding to each joint;

Or for each first joint, acquiring first sensing data acquired by a sensor corresponding to the first joint, determining the actual moment of the first joint according to the first sensing data, acquiring current values corresponding to each second joint respectively, and determining the actual moment corresponding to each second joint according to the mapping relation between the current and the moment;

for each joint, taking the difference value between the theoretical moment and the actual moment corresponding to the joint as the sub-external moment corresponding to the joint; and fusing the sub external moments corresponding to the joints respectively to obtain the external moment of the target instrument in the current period.

3. The method according to claim 1, wherein the determining the external torque of the target instrument in the current period according to the joint parameters and the target detection mode corresponding to each joint respectively includes:

acquiring current values corresponding to the joints respectively, and determining actual moments corresponding to the joints respectively according to the mapping relation between the current and the moment;

and determining the external torque of the target instrument in the current period through an impulse model according to the joint parameters corresponding to each joint and the actual torque corresponding to each joint.

4. The method according to claim 3, wherein determining the external torque of the target instrument in the current period according to the joint parameters corresponding to the joints and the actual torques corresponding to the joints respectively through the impulse model comprises:

determining impulse values corresponding to the joints respectively according to joint parameters corresponding to the joints respectively;

and determining at least one preamble period before the current period, determining external force moment corresponding to each preamble period, and determining the external force moment of the target instrument in the current period through an impulse model according to the external force moment corresponding to each preamble period, the actual force moment corresponding to each joint in the current period and the impulse value corresponding to each joint.

5. The method according to claim 1, wherein the acquiring the joint parameters corresponding to the respective joints of the target instrument in the current cycle includes:

acquiring a value of a contact force in a previous period, taking a period which is adjacent to and before the previous period as a target preamble period, and acquiring target parameters which respectively correspond to all joints of a target instrument in the target preamble period, wherein the target parameters comprise joint parameters;

Determining a measurement error according to the target parameters respectively corresponding to each joint of the target instrument in the target preamble period and the value of the contact force in the previous period;

acquiring measurement data which are fed back by a motor in a target instrument and respectively correspond to all joints in a current period;

and correcting the measurement data according to the measurement error to obtain target parameters corresponding to all the joints of the target instrument in the current period, and extracting joint parameters from the target parameters corresponding to all the joints of the target instrument in the current period.

6. The method according to claim 1, wherein the method further comprises:

under the condition that the using times of the target instrument is a using times threshold value, determining the value of the force to be compensated at the tail end of the target instrument according to the value of the contact force corresponding to the current period;

according to the value of the force to be compensated, determining a compensation moment corresponding to the force to be compensated in the current period through a mapping relation between the translational degree of freedom and the pose degree of freedom;

determining theoretical moment corresponding to each joint in the current period;

and determining a target moment according to the theoretical moment and the compensation moment respectively corresponding to each joint in the current period, wherein the target moment is used for enabling the target instrument to reach the expected position.

7. The method according to claim 2, wherein the method further comprises:

under the condition that the using times of the target instrument are the using times threshold value, the pose moment generated when each first joint moves under the driving of the motor is obtained;

acquiring theoretical moments corresponding to each joint in the current period, and determining translation moment based on the value of the contact force corresponding to the current period and the theoretical moment corresponding to each joint in the current period;

and determining a target moment according to the pose moment and the translation moment, wherein the target moment is used for enabling the target instrument to reach a desired position.

8. The method of claim 7, wherein determining the translational torque based on the value of the contact force corresponding to the current cycle and the theoretical torque corresponding to each joint in the current cycle, respectively, comprises:

determining the value of the force to be compensated at the tail end of the target instrument according to the value of the contact force corresponding to the current period;

according to the value of the force to be compensated, determining a compensation moment corresponding to the force to be compensated in the current period through a mapping relation of translational degrees of freedom;

and determining the translation moment according to the compensation moment and the actual moment corresponding to each joint in the current period.

9. The method according to any one of claims 1 to 8, further comprising:

and sending an alarm signal to the display equipment under the condition that the value of the contact force is greater than or equal to a threshold value.

10. A device for detecting an instrument contact force, the device comprising:

11. 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 steps of the method of any of claims 1 to 9.