Instrument contact force detection method, device, computer device, storage medium

Abstract

The application relates to a method and device for detecting instrument contact force, computer equipment, a storage medium and a computer program product. 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; determining an external moment of the target instrument in the current period according to the joint parameters corresponding to each joint and the target detection mode; determining a numerical value corresponding to the external moment and used for reflecting the force size according to a mapping relationship between the moment and the force, and taking the determined numerical value as the value of the contact force of the end of the target instrument in the current period. In this way, the precision of the contact force detection is improved.

Description

Technical Field

[0001] This application relates to the field of testing technology, and in particular to a method, apparatus, computer equipment, storage medium, and computer program product for testing the contact force of a device. Background Technology

[0002] With the development of medical device technology, in the process of repairing abnormalities in a target object, operators can remotely control a robotic arm to enable instruments mounted on the arm to perform repairs. During the repair process, the instruments will come into contact with the repair area, generating contact forces. To avoid excessive contact forces that could damage the repair area, it is necessary to monitor these contact forces.

[0003] In traditional techniques, contact force is often determined using force-sensing stamps or image observation instruments. However, when determining contact force using stamps, the contact force cannot be directly obtained and measured; a relatively long force transmission chain is required, which can easily introduce detection bias and result in low detection accuracy. In other words, there is a problem of low precision in detecting the contact force of instruments. Summary of the Invention

[0004] Therefore, it is necessary to provide a method, apparatus, computer equipment, computer-readable storage medium, and computer program product for detecting instrument contact force that can improve the accuracy of instrument contact force detection, in order to address the above-mentioned technical problems.

[0005] Firstly, this application provides a method for detecting the contact force of a device. The method includes:

[0006] Obtain the joint parameters corresponding to each joint of the target instrument within the current cycle; the joint parameters include the angle value and velocity value of the joint;

[0007] The target detection method is determined from a variety of preset detection methods;

[0008] Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined;

[0009] Based on the mapping relationship between torque and force, a value corresponding to the external torque and used to reflect the magnitude of the force is determined, and the determined value is used as the contact force value of the end of the target instrument in the current cycle.

[0010] Secondly, this application also provides a device for detecting the contact force of an instrument. The device includes:

[0011] The parameter acquisition module is used to acquire the joint parameters corresponding to each joint of the target instrument in the current cycle; the joint parameters include the angle value and velocity value of the joint.

[0012] The detection method determination module is used to determine the target detection method from a variety of preset detection methods;

[0013] The external torque determination module is used to determine the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method.

[0014] The contact force determination module is used to determine a value corresponding to the external torque and used to reflect the magnitude of the force based on the mapping relationship between torque and force, and to use the determined value as the contact force value of the end of the target instrument in the current cycle.

[0015] Thirdly, this application also provides a computer device. The computer device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the following steps:

[0016] Obtain the joint parameters corresponding to each joint of the target instrument within the current cycle; the joint parameters include the angle value and velocity value of the joint;

[0017] The target detection method is determined from a variety of preset detection methods;

[0018] Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined;

[0019] Based on the mapping relationship between torque and force, a value corresponding to the external torque and used to reflect the magnitude of the force is determined, and the determined value is used as the contact force value of the end of the target instrument in the current cycle.

[0020] Fourthly, this application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program thereon, which, when executed by a processor, performs the following steps:

[0021] Obtain the joint parameters corresponding to each joint of the target instrument within the current cycle; the joint parameters include the angle value and velocity value of the joint;

[0022] The target detection method is determined from a variety of preset detection methods;

[0023] Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined;

[0024] Based on the mapping relationship between torque and force, a value corresponding to the external torque and used to reflect the magnitude of the force is determined, and the determined value is used as the contact force value of the end of the target instrument in the current cycle.

[0025] Fifthly, this application also provides a computer program product. The computer program product includes a computer program that, when executed by a processor, performs the following steps:

[0026] Obtain the joint parameters corresponding to each joint of the target instrument within the current cycle; the joint parameters include the angle value and velocity value of the joint;

[0027] The target detection method is determined from a variety of preset detection methods;

[0028] Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined;

[0029] Based on the mapping relationship between torque and force, a value corresponding to the external torque and used to reflect the magnitude of the force is determined, and the determined value is used as the contact force value of the end of the target instrument in the current cycle.

[0030] The aforementioned method, apparatus, computer equipment, storage medium, and computer program product for detecting instrument contact force acquire joint parameters corresponding to each joint of the target instrument within the current cycle; these joint parameters include joint angle and velocity values. A target detection method is determined from a set of preset detection methods. Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target instrument within the current cycle is determined. That is, using the joint parameters corresponding to each joint within the current cycle, the external torque can be directly determined using the preset target detection method, effectively avoiding errors caused by indirect measurement. Based on the mapping relationship between torque and force, the value corresponding to the external torque and used to reflect the magnitude of the force can be quickly and accurately determined, and this determined value is used as the contact force value at the end of the target instrument within the current cycle. Therefore, by using the joint parameters corresponding to each joint of the target instrument within the current cycle and utilizing the target detection method, the contact force generated by the contact between the target instrument and the repair site can be directly and accurately detected, effectively avoiding measurement errors caused by indirect measurement, thereby improving the accuracy of instrument contact force detection. Attached Figure Description

[0031] Figure 1 This is a diagram illustrating the application environment of a method for detecting instrument contact force in one embodiment.

[0032] Figure 2 This is a flowchart illustrating a method for detecting the contact force of an instrument in one embodiment;

[0033] Figure 3 This is a schematic diagram of the target device structure in one embodiment;

[0034] Figure 4 This is a flowchart illustrating the process of determining the external torque in one embodiment;

[0035] Figure 5 This is a schematic diagram of the wire drive in one embodiment;

[0036] Figure 6 This is a schematic diagram illustrating the mapping relationship between current and torque in one embodiment;

[0037] Figure 7 This is a flowchart illustrating the process of determining the operating force in one embodiment;

[0038] Figure 8 This is a flowchart illustrating the determination of the external torque in another embodiment;

[0039] Figure 9 This is a schematic diagram of the sensor structure in one embodiment;

[0040] Figure 10 This is a schematic diagram of sensor installation in one embodiment;

[0041] Figure 11 This is a flowchart illustrating the process of determining the operating force in another embodiment;

[0042] Figure 12 This is a flowchart illustrating the process of determining the operating force in another embodiment;

[0043] Figure 13 This is a schematic diagram of a modified process in one embodiment;

[0044] Figure 14 This is a schematic diagram showing the location of the end effector of the target device in one embodiment;

[0045] Figure 15 This is a flowchart illustrating the steps for determining the target torque in one embodiment;

[0046] Figure 16 This is a schematic diagram of the target device in one embodiment;

[0047] Figure 17 This is a flowchart illustrating the steps for determining the target torque in another embodiment;

[0048] Figure 18 This is a schematic diagram of the structure of the operating table in one embodiment;

[0049] Figure 19 This is a schematic diagram of a display device in one embodiment;

[0050] Figure 20This is a structural block diagram of a device for detecting the contact force of an instrument in one embodiment;

[0051] Figure 21 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation

[0052] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0053] The instrument contact force detection method provided in this application embodiment can be applied to, for example... Figure 1 The application environment shown includes an operating workbench 100 for operator use, a repair workcart 200 for robotic arms to repair target objects, an image cart 300 for displaying images, a tool cart 400 for placing repair tools, and auxiliary components 500 for placing auxiliary tools. The operating workbench 100 is equipped with a main operator, and the repair workcart 200 includes at least two robotic arms 201, with instruments and endoscopes mounted on the robotic arms 201. The repair workcart 200 communicates with a computer device 600 via a network, and also communicates with the operating workbench 100 via the network. Figure 1 (Not shown in the image), the repair work trolley 200 communicates with the image trolley 300 via a network. Figure 1 (Not shown in the image). The computer device 600 can be a server or a terminal. The data storage system can store the data that the server needs to process. The data storage system can be integrated onto the server, or it can be located in the cloud or on other network servers.

[0054] In some embodiments, the master operator and the robotic arm 201 form a master-slave control relationship, and the master operator and the instrument also form a master-slave control relationship. Optionally, the operator remotely operates the workbench 100 and the master operator to perform abnormal repair operations on the target object. Exemplarily, the robotic arm 201 and the instrument move according to the movement of the master operator during the abnormal repair operation, that is, they operate according to the operation of the operator's hand. Further, if a contact force is generated between the repair site and the instrument, the master operator receives the force information of the repair site on the instrument and feeds it back to the operator's hand, so that the operator can have a more intuitive understanding of the abnormal repair operation. To facilitate the repair personnel to perform abnormal repair operations, the workbench 100 has a display screen, which is communicatively connected to the endoscope mounted on the robotic arm of the repair workbench 200, so that the display device can receive the images collected by the endoscope. According to the images displayed on the display device on the workbench 100 in the draft, the operator controls the movement of the robotic arm and the instrument through the master operator, so that the endoscope and the instrument can reach the repair site for abnormal repair.

[0055] In other embodiments, if the operator is unable to complete the corresponding operation through the main operator, or if an abnormal situation occurs during the abnormal repair operation, the intervention of the monitoring personnel is required, that is, the intervention operation is carried out through the image trolley 300.

[0056] In some embodiments, the computer device 600 acquires data collected by the target instrument in the repair worktable 200, and determines the joint parameters corresponding to each joint of the target instrument in the current cycle based on the acquired data. The computer device 600 acquires the joint parameters corresponding to each joint of the target instrument in the current cycle; the joint parameters include the angle value and velocity value of the joint. The computer device 600 determines a target detection method from a set of preset detection methods. Based on the joint parameters corresponding to each joint and the target detection method, the computer device 600 determines the external torque of the target instrument in the current cycle. Based on the mapping relationship between torque and force, the computer device 600 determines a value corresponding to the external torque and used to reflect the magnitude of the force, and uses the determined value as the contact force value at the end of the target instrument in the current cycle.

[0057] The terminal can be, but is not limited to, various personal computers, laptops, smartphones, tablets, and portable wearable devices. Portable wearable devices can include smartwatches, smart bracelets, head-mounted devices, etc. The server can be implemented using a standalone server or a server cluster consisting of multiple servers.

[0058] In one embodiment, such as Figure 2 As shown, a method for detecting mechanical contact force is provided, which can be applied to... Figure 1 Taking a computer device as an example, the explanation includes the following steps:

[0059] Step S202: Obtain the joint parameters corresponding to each joint of the target device in the current cycle; the joint parameters include the angle value and velocity value of the joint.

[0060] The target instrument is used for abnormal repair operations. It comprises multiple joints and can be either a continuous or discontinuous instrument, with no specific limitation. Because continuous instruments have multiple degrees of freedom and greater flexibility compared to discontinuous instruments, their contact forces are more difficult to determine. Compared to existing technologies, the accuracy of contact forces for ordinary instruments (i.e., discontinuous instruments) is lower, and accurate detection of contact forces for continuous instruments is also difficult. A structural diagram of a continuous instrument is shown below. Figure 3 The image shown is a schematic diagram of the target device structure in one embodiment. Figure 3 In this system, the joints near the end effector of the target instrument include pitch, rotation, and yaw joints. The movement of the end effector includes translational and positional movements along the x, y, and z directions. Positional movements include pitch, rotation, and yaw motions of the end effector. The target instrument's end effector movement is achieved by motor-driven control of multiple consecutive joints. The pitch joint is controlled by a motor driving a guidewire for pitch movement; the rotation joint is controlled by another motor driving a guidewire for rotational movement; and the yaw joint is controlled by two motors driving two yaw plates for yaw motion.

[0061] Optionally, the computer device acquires the joint parameters corresponding to each joint of the target instrument in the current cycle, including joints near the end of the target instrument and joints not near the end of the target instrument.

[0062] For example, for each joint, the data acquisition device on the motor of the targeted joint directly obtains the joint parameters of the targeted joint, and the computer device directly obtains the joint parameters sent by the data acquisition device corresponding to the targeted joint.

[0063] Furthermore, since the joint parameters acquired by the data acquisition device may contain errors, parameter correction can be performed on the joint parameters acquired by the data acquisition device to ensure the accuracy of the detection. Based on this, for example, for each joint, the data acquisition device on the motor of the targeted joint directly acquires the measurement data of the targeted joint, the computer device acquires the measurement data of the targeted joint, and performs data correction on the measurement data to obtain the joint parameters of the targeted joint. The measurement data includes the joint parameters to be corrected.

[0064] Step S204: Determine the target detection method from a variety of preset detection methods.

[0065] The detection method is used to determine the external torque of the target instrument, and then, based on the external torque, to determine the contact force generated when the target instrument comes into contact with the repair site. The external torque is a physical quantity that causes the target instrument to rotate; the external force can be directly determined based on the external torque, and in this application, the external force is the contact force.

[0066] The detection methods include a first detection method using the wire transmission principle, a second detection method using a stress sensor, and a third detection method using joint impulse.

[0067] Optionally, the computer device determines the target detection method from a set of preset, different detection methods.

[0068] For example, the computer device determines the target detection method of the previous cycle and directly uses the target detection method of the previous cycle as the target detection method of the current cycle.

[0069] For example, the computer device determines the target detection method from a variety of preset detection methods based on the needs of the current cycle.

[0070] It should be noted that the first, second, and third detection methods do not involve obtaining a long force transmission chain, nor do they involve determining the degree of deformation. Therefore, any of the preset detection methods involved in this application can directly obtain the external torque, that is, directly determine the contact force based on the external torque.

[0071] Step S206: Determine the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method.

[0072] As mentioned earlier, an external torque is a torque that can cause the target instrument to rotate. This external torque includes sub-torques corresponding to each joint, which can be mathematically represented as a vector containing the values ​​of the sub-torques corresponding to each joint.

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

[0074] It should be noted that when the target instrument contacts the repair site, if the target instrument does not exert any contact force with the repair site, or the contact force is extremely small and will not affect the target instrument or the repair site, the theoretical torque and the actual torque are the same. Once a contact force is generated, or a contact force that causes an impact is generated, there will be a difference between the theoretical torque and the actual torque. Based on this, the external torque is calculated.

[0075] When the target detection method is the third detection method that utilizes joint impulse, the computer equipment determines the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and through the impulse model.

[0076] For example, when the target detection method is a first detection method utilizing the wire transmission principle, or a second detection method utilizing a stress sensor, for each joint, the computer device, based on the joint parameters of the targeted joint, uses the target detection method to determine the theoretical torque and actual torque corresponding to the targeted joint, and calculates the difference between the theoretical torque and actual torque corresponding to the targeted joint. Based on the difference corresponding to each joint, the computer device determines the external torque of the target device in the current cycle.

[0077] When the target detection method is the third detection method using joint impulse, the computer equipment acquires the impulse model constructed based on the joint impulse, and determines the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint through the impulse model.

[0078] Step S208: Based on the mapping relationship between torque and force, determine the value corresponding to the external torque and used to reflect the magnitude of the force, and use the determined value as the contact force value of the end of the target device in the current cycle.

[0079] Optionally, the computer device acquires the mapping relationship between torque and force, and determines a value corresponding to the external torque and used to reflect the magnitude of the force based on the mapping relationship between torque and external force. The computer device uses the determined value as the contact force value of the end of the target instrument in the current cycle.

[0080] The mapping relationship between torque and force can be regarded as a Jacobian matrix used to determine the contact force. This Jacobian matrix represents the matrix from the Cartesian space corresponding to the end of the target instrument to the joint space.

[0081] As mentioned earlier, external torque can be considered as a vector, which includes the sub-external torque corresponding to each joint.

[0082] For example, the computer device obtains a Jacobian matrix for determining the contact force and the inverse matrix of the transpose of the Jacobian matrix. Based on the inverse matrix and the external torque, the computer device determines a value corresponding to the external torque and used to reflect the magnitude of the force.

[0083] For example, computer equipment determines the Jacobian matrix J and the external torque τ used to determine the contact force. ext Then, the contact force f is determined based on the following formula. ext :

[0084] f ext =(J T ) -1 *τ ext

[0085] In the aforementioned method for detecting instrument contact force, the joint parameters corresponding to each joint of the target instrument within the current cycle are obtained; these joint parameters include the angle and velocity values ​​of the joints. A target detection method is determined from a set of preset detection methods. Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target instrument within the current cycle is determined. That is, using the joint parameters corresponding to each joint within the current cycle, the external torque can be directly determined using the preset target detection method, effectively avoiding errors caused by indirect measurement. Based on the mapping relationship between torque and force, the value corresponding to the external torque and used to reflect the magnitude of the force can be quickly and accurately determined, and this determined value is used as the contact force value at the end of the target instrument within the current cycle. Therefore, by using the joint parameters corresponding to each joint of the target instrument within the current cycle and utilizing the target detection method, the contact force generated by the contact between the target instrument and the repair site can be directly and accurately detected, effectively avoiding measurement errors caused by indirect measurement, thereby improving the accuracy of instrument contact force detection.

[0086] In some embodiments, such as Figure 4 The diagram shown illustrates the process of determining the external torque in one embodiment. The joints include a first joint near the end of the target device and a second joint not near the end of the target device. The second joint is driven by a wire drive. Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined, including:

[0087] Step S402: Based on the joint parameters corresponding to each first joint, determine the theoretical torque corresponding to each first joint using the dynamic model corresponding to the non-wire transmission method.

[0088] As mentioned earlier, the joints of the target instrument include pitch, rotation, and yaw joints; that is, the first joint can be a pitch, rotation, or yaw joint. For the second joint, which is not near the end of the target instrument, the driving method is wire drive. In wire drive, there are two types of joints: a primary joint directly driven by a motor, and a secondary joint that is not driven by a motor, driven by a lead wheel corresponding to the primary joint. For example... Figure 5The diagram shown is a schematic of a wire drive in one embodiment. The wire wheel M is directly connected to the motor, and r... m Let A be the radius of the lead wheel M. m This represents the motor's inertia matrix. Lead wheel M is connected to lead wheel 1 via a guide wire, and lead wheel 2 is connected to lead wheel 1 via a guide wire. That is, the motor drives lead wheel M to rotate, and the rotation of lead wheel M drives the rotation of lead wheels 1 and 2. It should be noted that the joints mentioned in this application are the main joints, pitch joints, rotation joints, and yaw joints directly determined by the motor. The main joint is the second joint.

[0089] The dynamic model corresponding to non-wire transmission methods refers to the basic dynamic model, that is, the model that does not consider the wire transmission situation.

[0090] Optionally, the computer device acquires the joint parameters corresponding to each first joint and obtains the dynamic model corresponding to the non-wire transmission method. For each first joint, the computer device determines the theoretical torque corresponding to the first joint based on the joint parameters and the dynamic model corresponding to the non-wire transmission method. This theoretical torque is the theoretical torque of the first joint.

[0091] For example, for each first joint, the computer device acquires the angle value q and the velocity value of the corresponding first joint. For joints, angle values ​​can also be used as position values. (Based on velocity values) Determine acceleration Based on this, the computer device determines the theoretical torque τ of the joint being targeted using the dynamic model corresponding to the following non-wire transmission method:

[0092]

[0093] Where M(.) is the mass matrix, C(.) is the Coriolis force and centripetal force matrix, and G(.) is the gravity function used to obtain the gravity term.

[0094] Step S404: Based on the joint parameters corresponding to each second joint, determine the theoretical torque corresponding to each second joint using the dynamic model corresponding to the wire drive method.

[0095] As mentioned earlier, the second joint is driven by wire drive. The dynamic model corresponding to wire drive is a modified version of the basic dynamic model, taking into account the principle of wire drive, and is applicable to wire drive. This dynamic model corresponding to wire drive takes into account the characteristics of the wire drive and joint coupling motion of the target device, as well as the frictional torque of the joint of the target device.

[0096] Optionally, the computer device acquires the joint parameters corresponding to each second joint and obtains the dynamic model corresponding to the wire drive method. For each second joint, the computer device determines the theoretical torque corresponding to the second joint based on the joint parameters and the dynamic model corresponding to the wire drive method.

[0097] For example, for each second joint, the computer device acquires the angle value q and the velocity value of the corresponding second joint. For the second joint, the angle value can also be used as the position value. (Based on the velocity value) Determine acceleration like Figure 5 The second joint is the joint corresponding to the lead wheel M.

[0098] Based on this, the computer equipment determines the theoretical torque τ of the targeted joint using the following dynamic model corresponding to the wire drive method. M :

[0099]

[0100]

[0101] Where, τ m It is the output torque of the motor; r m A is the radius of the guide wire wheel, f is the tension on the guide wire; m It is the motor inertia matrix; q m It is the motor angle; C m It is the Coriolis and centripetal force matrix of the motor; F c It is the Coulomb friction parameter; F s It is the maximum static friction force; v s It is the speed of Stribeck; F v This is the viscous friction parameter. Sign(.) is the sign parameter.

[0102] Step S406: Obtain the current value corresponding to each joint. And / or, obtain the sensing data collected by the sensors corresponding to each joint.

[0103] The joints involved in step S406 include a first joint and a second joint. The sensor refers to a stress sensor, that is, a sensor used to acquire stress. Exemplarily, the sensor can be a miniature FBG (Fiber Bragg Grating) sensor. The sensing data includes the change in the reflection center wavelength of the grating in the sensor.

[0104] Optionally, the computer device acquires the current values ​​corresponding to each joint of the target device in the current cycle, that is, acquires the current values ​​corresponding to each first joint and each second joint.

[0105] For example, for each joint, the data acquisition device on the motor of the targeted joint directly acquires the current value of the targeted joint. The computer device directly acquires the current value sent by the data acquisition device corresponding to the targeted joint.

[0106] As mentioned earlier, the measurement data acquired by the data acquisition equipment may contain errors, and the measurement data also includes current values. Therefore, to ensure the accuracy of the detection, the measurement data needs to be corrected.

[0107] Based on this, exemplarily, for each joint, a data acquisition device on the motor of the targeted joint directly acquires the measurement data of the targeted joint, a computer device acquires the measurement data of the targeted joint, and performs data correction on the measurement data to obtain the current value of the targeted joint. The measurement data includes the joint parameters to be corrected.

[0108] Optionally, the computer device acquires the sensor data corresponding to each joint, that is, it acquires the sensor data corresponding to each first joint and the sensor data corresponding to each second joint.

[0109] Step S408: Determine the actual torque corresponding to each joint based on the mapping relationship between current and torque, and the current value corresponding to each joint.

[0110] Optionally, the computer device acquires the mapping relationship between current and torque. For each first joint, the computer device determines the actual torque corresponding to the first joint based on the mapping relationship between current and torque and the current value corresponding to the first joint. The actual torque is the actual torque. For each second joint, the computer device determines the actual torque corresponding to the second joint based on the mapping relationship between current and torque and the current value corresponding to the second joint.

[0111] The mapping relationship between current and torque is a piecewise linear relationship, meaning that torque and current are linearly related. For example... Figure 6 The diagram shown illustrates the mapping relationship between current and torque in one embodiment.

[0112] Step S410: For each first joint, acquire the first sensing data collected by the sensor corresponding to the first joint, and determine the actual torque of the first joint based on the first sensing data. Acquire the current value corresponding to each second joint, and determine the actual torque corresponding to each second joint based on the mapping relationship between current and torque.

[0113] Step S412: For each joint, the difference between the theoretical torque and the actual torque corresponding to the joint is taken as the sub-external torque corresponding to the joint.

[0114] Optionally, for each first joint, the computer device uses the difference between the theoretical torque and the actual torque of the targeted first joint as the corresponding sub-external torque of the targeted first joint. For each second joint, the computer device uses the difference between the theoretical torque and the actual torque of the targeted second joint as the corresponding sub-external torque of the targeted second joint.

[0115] For example, for any joint i, it can be either the first joint or the second joint. The computer device determines the theoretical torque τ corresponding to that joint. expi and theoretical torque τ fdbi Subsequently, the external torque τ corresponding to this joint exti Determined by the following formula:

[0116] τ ext i = τ exp i-τfdbi

[0117] Step S414: Combine the sub-external torques corresponding to each joint to obtain the external torque of the target instrument in the current cycle.

[0118] Optionally, the computer device combines the sub-external torques corresponding to each first joint and the sub-external torques corresponding to each second joint to obtain the external torque of the target instrument in the current cycle.

[0119] Based on the above steps S402, S404, S406, S408, and S414, a more detailed embodiment for determining the contact force is provided. In a more specific embodiment, such as Figure 7 The diagram shown is a flowchart illustrating the process of determining the operating force in one embodiment.

[0120] Specifically, for each joint, the computer device determines the angle and velocity values ​​of the target joint based on the angle and velocity values ​​fed back from the motor corresponding to that joint. For each first joint, the computer device determines the theoretical torque corresponding to the target first joint based on the angle and velocity values, using a dynamic model corresponding to the non-wire drive method. For each second joint, the computer device determines the theoretical torque corresponding to the target second joint based on the angle and velocity values, using a dynamic model corresponding to the wire drive method.

[0121] For each joint, the computer device determines the current value of the targeted joint based on the current value fed back from the motor corresponding to that joint. For each first joint, the computer device determines the actual torque corresponding to the targeted first joint based on the mapping relationship between current and torque, and the current value corresponding to the targeted first joint. For each second joint, the computer device determines the actual torque corresponding to the targeted second joint based on the mapping relationship between current and torque, and the current value corresponding to the targeted second joint. For each first joint, the computer device uses the difference between the theoretical torque and the actual torque of the targeted first joint as the sub-external torque corresponding to the targeted first joint. For each second joint, the computer device uses the difference between the theoretical torque and the actual torque of the targeted second joint as the sub-external torque corresponding to the targeted second joint. The computer device combines the sub-external torques corresponding to each first joint and each second joint to obtain the external torque of the target device in the current cycle. The computer device acquires the Jacobian matrix used to determine the contact force, and the inverse matrix of the transpose of the Jacobian matrix. Based on the inverse matrix and the external torque, the computer device determines the numerical value corresponding to the external torque and used to reflect the magnitude of the force.

[0122] In this embodiment, based on the driving method of the target device's joints, a corresponding dynamic model is determined, thereby obtaining a theoretical torque that matches the joint driving method. Simultaneously, the current value directly reflects the actual torque of each joint. Based on this, and using both the theoretical and actual torques, the corresponding external torque for each joint can be accurately determined, thus obtaining a highly accurate external torque for the target device.

[0123] As mentioned earlier, to ensure the accuracy of force detection and reduce costs, sensors can be installed only at the relevant positions of the first joint to obtain the first sensing data corresponding to the first joint, thereby determining the actual torque corresponding to the first joint. Correspondingly, based on the current value corresponding to the second joint, the actual torque corresponding to the second joint can be determined. Based on this, in some embodiments, such as... Figure 8 The diagram shown illustrates the process for determining the external torque in another embodiment. The joint includes 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 is driven by a wire drive.

[0124] Based on the joint parameters corresponding to each joint and the target detection method, determine the external torque of the target device in the current cycle, including:

[0125] Step S802: Based on the joint parameters corresponding to each first joint, determine the theoretical torque corresponding to each first joint using the dynamic model corresponding to the non-wire transmission method.

[0126] As mentioned earlier, the joints of the target instrument include pitch, rotation, and yaw joints; that is, the first joint is either a pitch joint, a rotation joint, or a yaw joint. For the second joint, which is not near the end of the target instrument, the driving method is wire drive. In wire drive, there are two types of joints: a primary joint directly driven by a motor, and a secondary joint that is not driven by a motor, driven by a lead wheel corresponding to the primary joint. For example... Figure 5 The diagram shown is a schematic of a wire drive in one embodiment. The wire wheel M is directly connected to the motor, and r... m Let A be the radius of the lead wheel M. m This represents the motor's inertia matrix. Lead wheel M is connected to lead wheel 1 via a guide wire, and lead wheel 2 is connected to lead wheel 1 via a guide wire. That is, the motor drives lead wheel M to rotate, and the rotation of lead wheel M drives the rotation of lead wheels 1 and 2. It should be noted that the joints mentioned in this application are the main joints, pitch joints, rotation joints, and yaw joints directly determined by the motor. The main joint is the second joint.

[0127] The dynamic model corresponding to non-wire transmission methods refers to the basic dynamic model, that is, the model that does not consider the wire transmission situation.

[0128] Optionally, the computer device acquires the joint parameters corresponding to each first joint and obtains the dynamic model corresponding to the non-wire transmission method. For each first joint, the computer device determines the theoretical torque corresponding to the first joint based on the joint parameters and the dynamic model corresponding to the non-wire transmission method. This theoretical torque is the theoretical torque of the first joint.

[0129] For example, for each first joint, the computer device acquires the angle value q and the velocity value of the corresponding first joint. For joints, angle values ​​can also be used as position values. (Based on velocity values) Determine acceleration Based on this, the computer device determines the theoretical torque τ of the joint being targeted using the dynamic model corresponding to the following non-wire transmission method:

[0130]

[0131] Where M(.) is the mass matrix, C(.) is the Coriolis force and centripetal force matrix, and G(.) is the gravity function used to obtain the gravity term.

[0132] Step S804: Based on the joint parameters corresponding to each second joint, determine the theoretical torque corresponding to each second joint using the dynamic model corresponding to the wire drive method.

[0133] As mentioned earlier, the second joint is driven by wire drive. The dynamic model corresponding to wire drive is a modified version of the basic dynamic model, taking into account the principle of wire drive, and is applicable to wire drive. This dynamic model corresponding to wire drive takes into account the characteristics of the wire drive and joint coupling motion of the target device, as well as the frictional torque of the joint of the target device.

[0134] Optionally, the computer device acquires the joint parameters corresponding to each second joint and obtains the dynamic model corresponding to the wire drive method. For each second joint, the computer device determines the theoretical torque corresponding to the second joint based on the joint parameters and the dynamic model corresponding to the wire drive method.

[0135] For example, for each second joint, the computer device acquires the angle value q and the velocity value of the corresponding second joint. For the second joint, the angle value can also be used as the position value. (Based on the velocity value) Determine acceleration like Figure 5 The second joint is the joint corresponding to the lead wheel M.

[0136] Based on this, the computer equipment determines the theoretical torque τ of the targeted joint using the following dynamic model corresponding to the wire drive method. M :

[0137]

[0138]

[0139]

[0140] Where, τ m It is the output torque of the motor; r m A is the radius of the guide wire wheel, f is the tension on the guide wire; m It is the motor inertia matrix; q m It is the motor angle; C m It is the Coriolis and centripetal force matrix of the motor; F c It is the Coulomb friction parameter; F s It is the maximum static friction force; v s It is the speed of Stribeck; F v This is the viscous friction parameter. Sign(.) is the sign parameter.

[0141] Step S806: For each first joint, acquire the first sensing data collected by the sensor corresponding to the first joint, and determine the actual torque of the first joint based on the first sensing data.

[0142] As mentioned earlier, the sensor refers to a stress sensor, that is, a sensor used to acquire stress. Exemplarily, the sensor can be a miniature FBG (Fiber Bragg Grating) sensor. Figure 9 The diagram shown is a schematic representation of a sensor in one embodiment. An optical fiber is positioned at the center of the sensor and is encased in a nickel-titanium alloy tube. Its sensing principle is based on the fact that the reflected center wavelength changes when a force is applied to the grating.

[0143] To facilitate effective detection of each first joint, corresponding sensor data is acquired for that first joint to determine its actual torque. For example... Figure 10 The diagram shown is a schematic of sensor installation in one embodiment. The two ends of the guide wire are fixed to the two ends of the sensor so that the sensor is suspended. If the guide wire connected to the first joint is subjected to force, the sensor will be compressed or stretched along the axial direction.

[0144] The first sensing data includes the change in the reflection center wavelength of the grating in the sensor.

[0145] Optionally, for each first joint, the computer device acquires first sensing data collected by a sensor corresponding to the first joint, and obtains the change in the reflection center wavelength from the first sensing data. Based on this change, the computer device determines the stress corresponding to the first joint, and based on this stress, determines the actual torque corresponding to the first joint.

[0146] For example, for each first joint, after the computer device determines the change in the reflection center wavelength of the grating corresponding to the first joint, the computer device determines the stress of the first joint based on the change in the reflection center wavelength and a first function, wherein the first function is a function of the change in the reflection center wavelength as a function of stress. The computer device determines the tension in the guide wire where the first joint is located based on the stress. The computer device determines the actual torque corresponding to the first joint based on the tension in the guide wire where the first joint is located and a second function, wherein the second function is a function of torque as a function of tension.

[0147] For example, the expression for the first function is as follows:

[0148] Δλ B =K σ *σ z

[0149] Where, Δλ B K represents the change in the wavelength of the reflection center of the grating. σσ represents the stress coefficient, which is a constant; z This represents the axial stress of the sensor. For example, the sensor has an outer diameter of 2 mm and a length of 7 mm, which facilitates abnormal repair operations on the target instrument within a confined space. The change in the reflection center wavelength is positively correlated with the stress.

[0150] The expression for the second function is as follows:

[0151] τ joint =J*f*r

[0152] Where, τ joint This represents the actual torque corresponding to the first joint being targeted. J represents the mapping matrix between the motor and the joint torque. f is the tension force, and r is the lever arm of the tension force to the shaft.

[0153] Step S808: Obtain the current value corresponding to each second joint, and determine the actual torque corresponding to each second joint according to the mapping relationship between current and torque.

[0154] Optionally, the computer device acquires the current value corresponding to each second joint within the current cycle, and determines the actual torque corresponding to each second joint based on the mapping relationship between current and torque. The actual torque is the actual torque.

[0155] For example, for each second joint, a data acquisition device directly drives the motor of that joint to acquire the current value of that second joint. The computer device directly acquires the current value sent by the data acquisition device corresponding to that second joint.

[0156] As mentioned earlier, the measurement data acquired by the data acquisition equipment may contain errors, and the measurement data also includes current values. Therefore, to ensure the accuracy of the detection, the measurement data needs to be corrected.

[0157] Based on this, exemplarily, for each second joint, a data acquisition device on the motor of the targeted second joint directly acquires the measurement data of the targeted second joint, a computer device acquires the measurement data of the targeted second joint, and performs data correction on the measurement data to obtain the current value of the targeted second joint. The measurement data includes the joint parameters to be corrected.

[0158] After obtaining the current values ​​corresponding to each second joint, the computer device determines the actual torque corresponding to each second joint based on the current values ​​corresponding to each second joint and the mapping relationship between current and torque.

[0159] Step S810: For each first joint, the difference between the theoretical torque and the actual torque corresponding to the first joint is taken as the sub-external torque of the first joint.

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

[0161] Step S812: For each second joint, the difference between the theoretical torque and the actual torque corresponding to the second joint is taken as the sub-external torque of the second joint.

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

[0163] Step S814: Combine the sub-external torques corresponding to each first joint and the sub-external torques corresponding to each second joint to obtain the external torque of the target device in the current cycle.

[0164] Optionally, the computer device combines the sub-external torques corresponding to each first joint and the sub-external torques corresponding to each second joint to obtain the external torque of the target instrument in the current cycle.

[0165] Based on the above steps S802 to S814, a more detailed embodiment for determining the contact force is provided. In a more specific embodiment, such as Figure 11 The diagram shown is a flowchart illustrating the process of determining the operating force in another embodiment.

[0166] Specifically, for each joint, the computer device determines the angle and velocity values ​​of the target joint based on the angle and velocity values ​​fed back from the motor corresponding to that joint. For each first joint, the computer device determines the theoretical torque corresponding to the target first joint based on the angle and velocity values, using a dynamic model corresponding to the non-wire drive method. For each second joint, the computer device determines the theoretical torque corresponding to the target second joint based on the angle and velocity values, using a dynamic model corresponding to the wire drive method.

[0167] For each first joint, the computer device acquires first sensing data collected by the sensor corresponding to the first joint, and obtains the change in the reflected center wavelength from the first sensing data. Based on this change, the computer device determines the stress corresponding to the first joint, and based on this stress, determines the actual torque corresponding to the first joint. For each second joint, the computer device determines the current value of the joint based on the current value fed back by the motor corresponding to the joint. The computer device acquires the current values ​​corresponding to each second joint in the current cycle, and determines the actual torque corresponding to each second joint based on the mapping relationship between current and torque. For each first joint, the difference between the theoretical torque and the actual torque corresponding to the first joint is used as the sub-external torque of the first joint. For each second joint, the difference between the theoretical torque and the actual torque corresponding to the second joint is used as the sub-external torque of the second joint. By fusing the sub-external torques corresponding to each first joint and each second joint, the external torque of the target device in the current cycle is obtained. The computer device acquires the Jacobian matrix used to determine the contact force, and the inverse matrix of the transpose of the Jacobian matrix. Based on the inverse matrix and the external torque, the computer device determines the numerical value corresponding to the external torque and used to reflect the magnitude of the force.

[0168] In this embodiment, a dynamic model corresponding to the joint driving method is determined based on the target instrument's joint driving method, thereby obtaining a theoretical torque that matches the joint driving method. Simultaneously, sensors deployed on the first joint monitor the tension on the guidewire in real time. Specifically, when the end of the target instrument experiences a contact force, the guidewire controlling the movement of the first joint within the target instrument experiences tension, enabling the sensors to collect sensing data. Based on this, the actual external torque of each first joint can be quickly and accurately determined. The current value directly reflects the actual torque of each second joint. Therefore, based on the theoretical torque and the actual torque, the sub-external torque corresponding to each joint can be accurately determined, thus obtaining a highly accurate external torque for the target instrument.

[0169] In some embodiments, determining the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method includes: acquiring the current value corresponding to each joint, and determining the actual torque corresponding to each joint based on the mapping relationship between current and torque. The external torque of the target device in the current cycle is determined based on the joint parameters corresponding to each joint and the actual torque corresponding to each joint, using an impulse model.

[0170] Optionally, the computer device acquires the current value corresponding to each joint and determines the actual torque corresponding to each joint based on the mapping relationship between current and torque. The computer device acquires an impulse model constructed based on joint impulse and determines the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the actual torque corresponding to each joint through the impulse model.

[0171] In this embodiment, the joints involved can be either the first joint or the second joint, without distinction. The impulse model is a model built based on the principle of an external force observer, that is, it is used to calculate torque based on impulse.

[0172] For example, after determining the actual torque corresponding to each joint, the computer device determines the impulse corresponding to each joint based on the joint parameters. Based on the impulse and the actual torque corresponding to the joint, the computer device uses an impulse model to determine the external torque of the target device in the current cycle.

[0173] For example, for joint i, determine the angle q and velocity corresponding to joint i. Then, the impulse p corresponding to joint i is calculated using the following formula:

[0174]

[0175] In this embodiment, the current value can intuitively reflect the actual torque of each joint. At the same time, based on the parameters corresponding to each joint and the actual torque corresponding to each joint, the external torque of the target device in the current cycle can be accurately obtained by directly using the impulse model.

[0176] In some embodiments, the external torque of the target device in the current cycle is determined based on the joint parameters corresponding to each joint and the actual torque corresponding to each joint, and through an impulse model. This includes: determining the impulse value corresponding to each joint based on the joint parameters corresponding to each joint; determining at least one preceding cycle before the current cycle, determining the external torque corresponding to each preceding cycle, and determining the external torque of the target device in the current cycle based on the external torque corresponding to the preceding cycle, the actual torque corresponding to each joint in the current cycle, and the impulse value corresponding to each joint, through an impulse model.

[0177] Optionally, the computer device acquires the joint parameters corresponding to each joint, and for each joint parameter, the computer device determines the impulse corresponding to the targeted joint based on the joint parameters corresponding to the targeted joint. The computer device determines one or more preceding cycles preceding the current cycle. The computer device determines the external torque corresponding to each preceding cycle. Based on the external torque corresponding to the preceding cycles, the actual torque corresponding to each joint in the current cycle, and the impulse value corresponding to each joint, the computer device determines the external torque of the target device in the current cycle using an impulse model.

[0178] For example, after determining the impulse corresponding to each joint, the computer device determines all preceding cycles and the corresponding external torque for each preceding cycle. The computer device acquires the actual torque for each preceding cycle. Based on the external torque, actual torque, and joint parameters of the preceding cycles, the computer device performs integral calculations using the impulse model to determine the integral value. The computer device acquires the actual torque for the current cycle and uses the preceding cycle adjacent to the current cycle as the previous cycle. Based on the actual torque, joint parameters, and external torque of the previous cycle, the computer device calculates the current value using the impulse model. The computer device merges the impulse values ​​corresponding to each joint to obtain an impulse vector. Using the impulse model, the computer device subtracts the integral value from the impulse vector in the current cycle and then subtracts the current value from the impulse vector in the current cycle to determine the difference. Based on this difference, the computer device determines the external torque of the target device in the current cycle.

[0179] For example, the external torque of the target instrument during the current cycle can be calculated using the following formula:

[0180]

[0181] Where, τ ext The torques are the internal and external torques for the current cycle, K is a constant, and p is the impulse value. T1 is the current cycle, and T1-1 is the previous cycle. C is the actual torque corresponding to the preceding period t. t Here are the Coriolis force and centripetal force matrices for the preceding period t. G represents the velocity value corresponding to the preceding period t. t This is the gravity term corresponding to the preceding period t, calculated based on the joint parameters corresponding to the preceding period t. This represents the external torque corresponding to the preceding period t. C is the actual torque corresponding to the current period T1. T1 This is the Coriolis force and centripetal force matrix corresponding to the current period. G represents the velocity value corresponding to the current cycle. T1This is the gravity term corresponding to the current cycle, calculated based on the joint parameters corresponding to the current cycle. This is the external torque corresponding to the previous cycle.

[0182] Furthermore, a more detailed embodiment for determining the contact force is provided. In a more specific embodiment, such as Figure 12 The diagram shown is a flowchart illustrating the process of determining the operating force in another embodiment.

[0183] Specifically, for each joint, the computer device determines the angle and velocity values ​​of the targeted joint based on the angle and velocity values ​​fed back from the motor corresponding to that joint. For each joint, the computer device determines the current value of the targeted joint based on the current value fed back from the motor corresponding to that joint. The computer device determines the actual torque corresponding to the targeted joint based on the mapping relationship between current and torque, and the current value corresponding to the targeted joint. The computer device determines the impulse value corresponding to each joint based on the joint parameters corresponding to each joint; it determines at least one preceding cycle, and determines the external torque corresponding to each preceding cycle; and based on the external torque corresponding to the preceding cycles, the actual torque corresponding to each joint in the current cycle, and the impulse value corresponding to each joint, it determines the external torque of the target device in the current cycle using an impulse model. The computer device obtains the Jacobian matrix used to determine the contact force, and the inverse matrix of the transpose of this Jacobian matrix; based on this inverse matrix and the external torque, the computer device determines the numerical value corresponding to the external torque and used to reflect the magnitude of the force.

[0184] In this embodiment, the current value can intuitively reflect the actual torque of each joint. Based on this, in determining the external torque in the current cycle, the impulse model is used, combined with the actual torque and external torque corresponding to the previous cycle and the actual torque of the current cycle, to accurately evaluate the external torque of the target device in the current cycle.

[0185] As mentioned earlier, the measurement data obtained based on the motor may contain errors. To ensure the accuracy of the subsequent contact force, the measurement data obtained in the current cycle can be calibrated in advance before determining the contact force in the current cycle based on the measurement data obtained in the current cycle. This can greatly improve the effectiveness and reliability of the contact force.

[0186] Therefore, in some embodiments, obtaining the joint parameters corresponding to each joint of the target device in the current cycle includes: obtaining the contact force value in the previous cycle, taking the cycle adjacent to and preceding the previous cycle as the target preceding cycle, and obtaining the target parameters corresponding to each joint of the target device in the target preceding cycle, including the joint parameters. Based on the target parameters corresponding to each joint of the target device in the target preceding cycle and the contact force value in the previous cycle, a measurement error is determined; measurement data corresponding to each joint in the current cycle is obtained from the motor feedback in the target device. The measurement data is corrected based on the measurement error to obtain the target parameters corresponding to each joint of the target device in the current cycle, and the joint parameters are extracted from the target parameters corresponding to each joint of the target device in the current cycle.

[0187] Among them, the period between the current period and the target preceding period is the previous period. The target parameters corresponding to each joint in the target preceding period refer to the parameters after data correction. The target parameters include the joint parameters and current values ​​corresponding to each joint in the target preceding period.

[0188] As previously stated, the contact force in the current cycle is determined based on the joint parameters in the current cycle. The following linearization of the state equations can be used to briefly describe how the contact force in the previous cycle is determined based on the target parameters corresponding to each joint in the preceding cycle:

[0189]

[0190] x t Let x be the target state corresponding to the target parameter in the previous period t, that is, let x represent the value of the corrected target parameter. t-1 For the target parameters within the preceding period t-1, z represents the target state. t The value of the contact force in the previous period t, and the noise term δ t ,∈ t All parameters are assumed to be Gaussian distributions with zero mean. F represents the mapping relationship between the corrected target parameters in the previous period and the target parameters in the previous period. H represents the mapping relationship between the corrected target parameters in the previous period and the contact force in the previous period. The above process is a correction process based on the Kalman filter principle.

[0191] Optionally, the computer device acquires the contact force value of the previous cycle and takes the cycle adjacent to and preceding the previous cycle as the target preceding cycle, and acquires the target parameters corresponding to each joint of the target device within the target preceding cycle. The computer device determines the measurement error based on the target parameters corresponding to each joint of the target device within the target preceding cycle and the contact force value of the previous cycle. The computer device acquires the measurement data corresponding to each joint of the target device within the current cycle, fed back from the motor in the target device; corrects the measurement data according to the measurement error to obtain the target parameters corresponding to each joint of the target device within the current cycle, and extracts the joint parameters from the target parameters corresponding to each joint of the target device within the current cycle. Wherein, when the current cycle is the first cycle, the target parameters corresponding to each joint of the target device within the target preceding cycle and the contact force value of the previous cycle are preset parameters and preset contact force values, respectively.

[0192] The measurement data consists of uncorrected data for each joint within the current cycle; the target parameters are obtained after correcting the original measurement data. The measurement data includes uncorrected joint parameters and current values.

[0193] For example, such as Figure 13 The diagram illustrates a modified process in one embodiment. The computer device determines the target test method from multiple preset first, second, and third test methods. Based on the target test method, it determines the contact force value in the previous cycle. Then, the computer device performs Kalman filtering calculations, determining the measurement error based on the target parameters corresponding to each joint of the target device in the previous cycle and the contact force value in the previous cycle. Based on the measurement error, the computer device updates (corrects) the measurement data corresponding to each joint in the current cycle, obtaining the target parameters corresponding to each joint of the target device in the current cycle. It then extracts the joint parameters from these target parameters to determine the contact force in the current cycle and outputs the contact force in the current cycle. If the current cycle is the first cycle, before determining the measurement error, the computer device obtains the preset parameters corresponding to each joint as the target parameters corresponding to each joint in the previous cycle, and obtains the preset contact force value as the contact force value in the previous cycle. Then, it performs calculations for the next cycle.

[0194] Specifically, by fusing the target parameters corresponding to each joint parameter within the target's preceding period t-1, an estimate of the target's preceding period t-1 is obtained. The following formula is used to estimate the preceding period t-1 of the target. Process the data to determine the expected value corresponding to the previous period's t and the estimated value.

[0195]

[0196] Then, the expected parameters of the covariance matrix corresponding to the previous period t are updated using the following formula 2.

[0197]

[0198] P t Let be the parameter matrix corresponding to the previous period t. Then, determine the Kalman coefficient matrix K corresponding to the previous period using the following formula (3). t :

[0199]

[0200] In Formula 2 above, Q and R in Formula 3 are both parameters corresponding to the covariance matrix. The estimated value P of the current period's covariance matrix is ​​then determined using Formula 4 below. t+1 (Calculation of measurement error used to determine the next cycle):

[0201]

[0202] In Formula 4 above, I represents the identity matrix. Finally, the estimation error corresponding to the previous period is determined based on Formula 5 below.

[0203]

[0204] In formula five above, z t The contact force value corresponding to the previous cycle t, and the estimation error corresponding to the previous cycle. This is the measurement error for the current cycle.

[0205] It should be noted that this embodiment corrects all values ​​in the measurement data synchronously in parallel. Of course, it is also possible to correct each value in the measurement data sequentially using the above process.

[0206] In this embodiment, by using the contact force value in the previous cycle and the target parameters corresponding to each joint in the target preceding cycle, the measurement error used for correction processing in the current cycle can be determined. Based on this, the measurement data obtained in the current cycle can be pre-calibrated, thereby greatly improving the effectiveness and reliability of the contact force.

[0207] As the number of abnormal repair operations increases, the target instrument may become damaged, especially when the preset usage threshold is reached. In such cases, the target instrument may experience wire loosening or even breakage, preventing it from reaching the intended position. Figure 14The diagram shown illustrates the location of the end of the target device in one embodiment. When the number of times the target device is used has not reached the usage threshold, the operator can make the target device reach the desired position through master-slave control, that is, the actual position of the end of the target device is the desired position. When the number of times the target device is used reaches the usage threshold, the operator needs to make the target device reach the desired position through master-slave control, but in reality, the actual position reached by the end of the target device does not coincide with the desired position.

[0208] Based on this, in some embodiments, the method further includes: when the number of uses of the target device is a threshold number of uses, determining the value of the force to be compensated at the end of the target device based on the value of the contact force corresponding to the current cycle; determining the compensation torque corresponding to the force to be compensated in the current cycle based on the value of the force to be compensated, through the mapping relationship between translational degrees of freedom and pose degrees of freedom; determining the theoretical torque corresponding to each joint in the current cycle; and determining the target torque based on the theoretical torque and compensation torque corresponding to each joint in the current cycle, the target torque being used to propel the target device to the desired position.

[0209] Since each target device has a service life, the service life is characterized by a usage threshold for each target device. For example, the usage threshold can represent the last number of uses, the last two uses, or the last three uses, etc., without any specific limitation. For example, if the service life is 5 uses, the usage threshold is 5, and the last use is the 5th time.

[0210] Optionally, when the number of uses of the target device is within a usage threshold, the computer device determines the value of the force to be compensated at the end of the target device based on the contact force value corresponding to the current cycle and through a preset gain coefficient. Based on the value of the force to be compensated, the computer device determines the compensation torque corresponding to the force to be compensated within the current cycle through the mapping relationship between translational and pose degrees of freedom. The computer device determines the theoretical torque corresponding to each joint within the current cycle. The computer device integrates the theoretical torque and compensation torque corresponding to each joint within the current cycle to determine the target torque.

[0211] For example, such as Figure 15 The diagram shown is a flowchart illustrating the steps for determining the target torque in one embodiment. Figure 15 The theoretical torque is determined using either the first or second detection method. Alternatively, it can be determined using a third detection method, which involves determining the target torque for the current cycle and then fusing the target torque with the actual torque to obtain the theoretical torque for that cycle.

[0212] The computer device determines the theoretical torque for each joint based on its joint parameters and the corresponding dynamic model. It then fuses these theoretical torques to determine the corresponding theoretical torque vector. Specifically, when the joint is the first joint, the corresponding dynamic model is the one corresponding to the non-wire-driven method; when the joint is the second joint, the corresponding dynamic model is the one corresponding to the wire-driven method. The computer device uses the product of the contact force and the gain coefficient as the value of the force to be compensated. Through the mapping relationship between translational and pose degrees of freedom, it determines the compensation torque corresponding to the force to be compensated within the current cycle. It then fuses the compensation torque and the theoretical torque to obtain the target torque. Based on the target torque, the joint controller controls the target device to reach the desired position.

[0213] In the process of determining the theoretical torque through the corresponding dynamic model, that is, for each joint, the acceleration value is determined based on the velocity value of that joint, and the angle value, velocity value, and acceleration value of that joint are input into the dynamic model corresponding to the joint to obtain the gravitational torque τ. g Inertial torque τ I and frictional torque τ f ,in:

[0214] τ g =G(q)

[0215]

[0216] Based on this, the process of determining the theoretical torque τ can be simplified to determining the gravitational torque τ. g Inertial torque τ I and frictional torque τ f To superimpose, that is:

[0217] τ=τ I +τ g +τ f

[0218] Among them, for the compensation torque τ ctrl In the process of determining the force, the force to be compensated, f, is determined using the following formula. ctrl :

[0219] f ctrl =-sign(f ext )Kf ext

[0220] Where sign(.) is the sign function, f ext Let K be the value of the contact force, and K be the gain coefficient. Then, the mapping relationship between translational and pose degrees of freedom can be considered as a Jacobian matrix J containing information about 6 degrees of freedom, i.e.:

[0221] τ ctrl =J T f ctrl

[0222] In this embodiment, the compensation torque is determined by the mapping relationship between translational and pose degrees of freedom, thereby obtaining a torque that characterizes translational and pose movements. Based on this, the target torque determined by the compensation torque and the theoretical torque can directly control the six degrees of freedom. Thus, even when the number of uses of the target instrument reaches a threshold, the problem of inaccurate control precision and decreased controllability caused by damage to the target instrument can be improved, thereby ensuring the accuracy of abnormal repair operations.

[0223] In some embodiments, the method further includes: acquiring the pose torque generated by each first joint during motor-driven movement when the number of uses of the target instrument is a threshold number; acquiring the theoretical torque corresponding to each joint in the current cycle; and determining the translational torque based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint in the current cycle; and determining the target torque based on the pose torque and translational torque, wherein the target torque is used to propel the target instrument to the desired position.

[0224] Optionally, when the number of uses of the target device is a threshold number, the computer device acquires the pose torque generated by each first joint during motor-driven movement and acquires the theoretical torque corresponding to each joint in the current cycle. Based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint in the current cycle, the computer device determines the translational torque. The computer device integrates the pose torque and the translational torque to determine the target torque.

[0225] It should be noted that, as Figure 16 The diagram illustrates the target device in one embodiment. Rotational control is achieved through motors for the pitch, rotation, and yaw joints, controlling three degrees of freedom (DOF). Force control is performed using the contact force within the current cycle, determining whether the current contact force results in excessive clamping force in the target device's gripping state. Since clamping force control involves the x, y, and z translational directions, it controls three translational DOFs. Based on this, by integrating the translational torque corresponding to the translational DOF and the pose torque corresponding to the pose DOF, control of all six DOFs at the end of the target device can be achieved. Furthermore, it ensures that even when the target device reaches the desired position, the clamping force is not excessive, thus preventing damage to the target device.

[0226] For example, such as Figure 17The diagram shows a flowchart of the step for determining the target torque in another embodiment. When the number of uses of the target device is within a threshold, if the target device did not reach the desired position or the desired contact force in the previous cycle (in which case the desired contact force will not damage the target device or the repair area), the computer device acquires the pose torque generated by each first joint during motor-driven movement for rotational control. Simultaneously, based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint in the current cycle, the computer device determines the translational torque to control the force in the translational direction. The computer device integrates the pose torque and translational torque to determine the target torque, and controls the operation of the target device using the target torque. After the target device completes its operation, the computer device acquires the actual position of the target device after completion. If the actual position is not the desired position, rotational control and force control continue in the next cycle. Alternatively, after the target device completes its operation, the computer device determines the environment in which the target device is located, i.e., whether the clamping force of the target device is too large. If it is too large, it will damage the target device. If the clamping force is too large, rotational control and force control continue in the next cycle.

[0227] In this embodiment, when the number of times the target device is used is within a threshold, by determining the translational torque corresponding to the translational degree of freedom and the pose torque corresponding to the pose degree of freedom respectively, it is possible not only to ensure that the target device reaches the expected position, but also to monitor in real time that the clamping force of the target device is not excessive. Therefore, while avoiding damage to the target device, it is also ensured that the target device accurately reaches the expected position.

[0228] In one embodiment, determining the translational torque based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint within the current cycle includes: determining the value of the force to be compensated at the end of the target instrument based on the contact force value corresponding to the current cycle; determining the compensation torque corresponding to the force to be compensated within the current cycle based on the value of the force to be compensated through the mapping relationship of the translational degrees of freedom; and determining the translational torque based on the compensation torque and the theoretical torque corresponding to each joint within the current cycle.

[0229] Optionally, the computer device determines the value of the force to be compensated at the end of the target device based on the value of the contact force corresponding to the current cycle, using a preset gain coefficient. Based on the value of the force to be compensated, the computer device determines the compensation torque corresponding to the force to be compensated within the current cycle through the mapping relationship of translational degrees of freedom. The computer device integrates the theoretical torque and compensation torque corresponding to each joint within the current cycle to determine the translational torque.

[0230] For example, the computer device calculates the product of the contact force value corresponding to the current cycle and a preset gain coefficient, and determines the value of the force to be compensated at the end of the target device based on this product. According to the value of the force to be compensated, the computer device determines the compensation torque corresponding to the force to be compensated within the current cycle through the mapping relationship of translational degrees of freedom. The computer device integrates the theoretical torque and compensation torque corresponding to each joint within the current cycle to determine the translational torque.

[0231] Specifically, the compensating force f ctrl It can be determined using the following formula:

[0232] f ctrl =-sign(f ext )Kf ext

[0233] Where sign(.) is the sign function, f ext Where is the value of the contact force, and K is the gain coefficient.

[0234] In this embodiment, when the number of times the target instrument is used is the threshold number of times it is used, the compensation torque in the current cycle can be accurately determined by the mapping relationship of the translational degrees of freedom. Thus, the translational torque in the current cycle can be determined, which is beneficial for effectively detecting whether the clamping force of the target instrument is too large in the future.

[0235] In some embodiments, the method further includes: issuing an alarm signal to a display device when the value of the contact force is greater than or equal to a threshold.

[0236] The display device can be a screen on the workbench or a screen on the image trolley. For example... Figure 18The diagram shows a schematic of the operating workbench in one embodiment. The operating workbench includes: an adjustment component 110, manipulator arms 120, a trolley component 130, and an image component 140 (i.e., a display screen). The two manipulator arms 120 detect the operator's hand movements via control handles at their ends, serving as motion control inputs. The trolley component 130 is a base support for mounting other components; it has movable casters for movement or fixation as needed. A foot switch is installed on the trolley component 130 to detect on / off control signals from the operator. The adjustment component 110 electrically adjusts the positions of the manipulator arms, image component, operator handrails, etc., providing human-machine parameter adjustment. The image component 140 provides the operator with stereoscopic images detected from the image system, offering reliable image information for abnormal repair operations. During abnormal repair operations, the operator sitting at the operating workbench is outside the disinfection area. The operator controls the target instrument and laparoscope by operating the control handles at the ends of the manipulator arms. Operators observe the transmitted intracavitary images through the imaging unit, and use their hands to control the movement of the robotic arm and target instruments on the repair worktable to complete various operations, thereby performing abnormal repair operations. At the same time, operators can control some actions through foot switches, such as inputting related operations such as electrocautery and electrocoagulation.

[0237] Optionally, the computer device acquires the value of the contact force and compares it with a threshold. If the contact force value is greater than or equal to the threshold, an alarm signal is issued to the display screen on the operating table and the display screen on the image trolley.

[0238] It should be noted that the threshold is used for alarms. Once the contact force value is not less than the threshold, it indicates that the contact between the target instrument and the repair area will cause damage to both the repair area and the end of the target instrument, requiring appropriate personnel intervention. Simultaneously, the display screen on the operating platform will also show the lifespan of the target instrument and the contact force value, such as... Figure 19 The diagram shown is a schematic of a display device in one embodiment. The display device shows the target instrument as a bipolar duckbill pliers with a service life of 5 cycles and a contact force value of 1.2N in the current cycle. Based on the numbers displayed on the device, the contact force at the tip can be adjusted to protect the target instrument from excessive external force and improve its reliability. Furthermore, as... Figure 18 As shown, once the contact force value is not less than the threshold, the operating table will also send a command to control the target device to stop moving in the direction that increases the contact force, so as to avoid damage to the target device due to excessive external force.

[0239] For example, the computer device performs contact force detection by comparing the contact force with a warning threshold. If the contact force does not exceed the warning threshold (i.e., the contact force is less than the warning threshold), the contact force for the next cycle is determined. If the contact force exceeds the warning threshold (i.e., the contact force is not less than the warning threshold), the computer device feeds back the contact force to the main terminal (operating workbench), allowing the operator to feel the corresponding force and prompting the operator to pay attention. If the contact force exceeds the threshold (i.e., the contact force is not less than the threshold, and the threshold is greater than the warning threshold), the computer device sends an alarm signal to the display screen on the operating workbench and the 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 terminal (operating workbench), allowing the operator to feel the corresponding force and prompting the operator to continue paying attention.

[0240] The notification threshold is used to provide a notification, but not to issue an alarm. The notification threshold is less than a specific threshold; for example, the notification threshold is 1 and the specific threshold is 2. When the contact force exceeds the notification threshold, or exceeds the notification threshold but is less than the specific threshold, the computer device, based on master-slave control and the contact force, determines the feedback force and sends the corresponding feedback torque to the master (operating console). This allows the operator to feel the corresponding force, thereby reducing the difficulty of operation and improving the safety of anomaly repair operations. For example:

[0241] f master =scale*f slave

[0242] T master =(J0) T )*f master

[0243] In the above formula, f slave The value of the contact force within the current cycle, scale is the mapping ratio, f master It is the feedback force, J0 is the Jacobian matrix of the robotic arm corresponding to the operating table, and τ master It is the expected feedback torque of the robotic arm corresponding to the operating table.

[0244] In this embodiment, when the contact force is greater than or equal to the threshold, an alarm signal is sent to the display device, thereby providing effective safety protection for the target instrument and the repair site.

[0245] In one embodiment, to facilitate a clearer understanding of the technical solution of this application, a more detailed embodiment is provided for description. The computer device includes a force sensing unit, a sensor data acquisition unit, a sensor data processing unit, an instrument control unit, a contact force detection unit, and an instrument operation force safety protection unit. The force sensing unit includes a sensor, a motor feedback current acquisition unit, a motor, and a joint position encoder. The sensor data acquisition unit acquires real-time sensor data. The sensor data processing unit processes the sensor data acquired by the sensor acquisition unit, converting the data into the magnitude of the guidewire force. The instrument control unit issues movement commands to the slave instrument based on the master hand's movement information, achieving master-slave following. The contact force detection unit calculates the instrument contact force value according to the target detection method. The instrument operation force safety protection unit allows the operator at the master end to feel the force on the end of the target instrument through master-slave force feedback, and issues an alarm via image when the contact force is too large, preventing damage to the target instrument or repair area due to excessive contact force.

[0246] Specifically, based on the operating workbench and the repair workcart, a master-slave operation is performed, that is, the computer equipment sets the operating workbench as the master and the repair workcart as the slave, and performs master-slave mapping so that the operating workbench and the repair workcart can interact with each other.

[0247] The contact force detection unit in the computer device acquires the contact force value from the previous cycle and designates the cycle adjacent to and preceding the previous cycle as the target preceding cycle. It then acquires the target parameters corresponding to each joint of the target device within the target preceding cycle, including joint parameters. Based on the target parameters corresponding to each joint of the target device within the target preceding cycle and the contact force value from the previous cycle, the measurement error is determined. The unit acquires the measurement data corresponding to each joint within the current cycle, fed back from the motor in the target device. The measurement data is corrected based on the measurement error to obtain the target parameters corresponding to each joint of the target device within the current cycle, and the joint parameters are extracted from these target parameters.

[0248] The joint includes 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 is driven by a wire drive.

[0249] If the target detection method is the first detection method, the contact force detection unit in the computer device determines the theoretical torque corresponding to each first joint based on the joint parameters corresponding to each first joint and through a dynamic model corresponding to the non-wire transmission method. Based on the joint parameters corresponding to each second joint, the theoretical torque corresponding to each second joint is determined using a dynamic model corresponding to the wire transmission method. The current value corresponding to each joint is obtained. Based on the mapping relationship between current and torque, and the current value corresponding to each joint, the actual torque corresponding to each joint is determined. For each joint, the difference between the theoretical torque and the actual torque corresponding to that joint is taken as the sub-external torque corresponding to that joint. The sub-external torques corresponding to each joint are fused to obtain the external torque of the target device in the current cycle.

[0250] If the target detection method is the second detection method, the contact force detection unit in the computer device determines the theoretical torque corresponding to each second joint based on the joint parameters corresponding to each second joint and through the dynamic model corresponding to the wire drive method. For each first joint, the first sensing data collected by the sensor corresponding to the first joint is acquired, and the actual torque of the first joint is determined based on the first sensing data. The current value corresponding to each second joint is acquired, and the actual torque corresponding to each second joint is determined based on the mapping relationship between current and torque. For each first joint, the difference between the theoretical torque and the actual torque corresponding to the first joint is taken as the sub-external torque of the first joint. For each second joint, the difference between the theoretical torque and the actual torque corresponding to the second joint is taken as the sub-external torque of the second joint. The sub-external torques corresponding to each first joint and each second joint are fused to obtain the external torque of the target device in the current cycle.

[0251] If the target detection method is the second detection method, the contact force detection unit in the computer device acquires the current value corresponding to each joint, and determines the actual torque corresponding to each joint based on the mapping relationship between current and torque. Based on the joint parameters corresponding to each joint, the impulse value corresponding to each joint is determined. At least one preceding cycle is identified, and the external torque corresponding to each preceding cycle is determined. Based on the external torque corresponding to the preceding cycle, the actual torque corresponding to each joint in the current cycle, and the impulse value corresponding to each joint, the external torque of the target device in the current cycle is determined using the impulse model.

[0252] After determining the external torque of the target instrument, the contact force detection unit in the computer equipment determines a value corresponding to the external torque and used to reflect the magnitude of the force based on the mapping relationship between torque and force, and uses the determined value as the contact force value of the end of the target instrument in the current cycle.

[0253] When the number of uses of the target instrument reaches a threshold, the contact force detection unit in the computer device determines the value of the force to be compensated at the end of the target instrument based on the contact force value corresponding to the current cycle. Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated within the current cycle is determined through the mapping relationship between translational degrees of freedom and pose degrees of freedom. The theoretical torque corresponding to each joint within the current cycle is determined. Based on the theoretical torque and compensation torque corresponding to each joint within the current cycle, the target torque is determined, and this target torque is used to propel the target instrument to the desired position.

[0254] Alternatively, when the number of uses of the target device reaches a threshold, the contact force detection unit in the computer device acquires the pose torque generated by each first joint during motor-driven movement; acquires the theoretical torque corresponding to each joint in the current cycle, and determines the value of the force to be compensated at the end of the target device based on the contact force value corresponding to the current cycle. Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated in the current cycle is determined through the mapping relationship of translational degrees of freedom. Based on the compensation torque and the actual torque corresponding to each joint in the current cycle, the translational torque is determined. Based on the pose torque and translational torque, the target torque is determined, and the target torque is used to propel the target device to the desired position.

[0255] The contact force detection unit in the computer device determines whether the contact force exceeds a threshold (i.e., whether it is greater than or equal to the threshold). If the contact force is greater than or equal to the threshold, the safety protection unit in the computer device sends feedback to the main unit and issues an alarm signal to the display device. If the contact force is greater than or equal to the threshold, the computer device continues the detection cycle.

[0256] In this embodiment, the joint parameters corresponding to each joint of the target instrument within the current cycle are obtained; the joint parameters include the angle and velocity values ​​of the joints. A target detection method is determined from a set of preset detection methods. Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target instrument within the current cycle is determined. That is, using the joint parameters corresponding to each joint within the current cycle, the external torque can be directly determined using the preset target detection method, effectively avoiding errors caused by indirect measurement. Based on the mapping relationship between torque and force, the value corresponding to the external torque and used to reflect the magnitude of the force can be quickly and accurately determined, and this determined value is used as the contact force value at the end of the target instrument within the current cycle. Therefore, by using the joint parameters corresponding to each joint of the target instrument within the current cycle and utilizing the target detection method, the contact force generated by the contact between the target instrument and the repair site can be directly and accurately detected, effectively avoiding measurement errors caused by indirect measurement, thereby improving the accuracy of instrument contact force detection.

[0257] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages of other steps.

[0258] Based on the same inventive concept, this application also provides a device for detecting instrument contact force to implement the aforementioned method for detecting instrument contact force. The solution provided by this device is similar to the solution described in the above method; therefore, the specific limitations in one or more embodiments of the device for detecting instrument contact force provided below can be found in the limitations of the method for detecting instrument contact force described above, and will not be repeated here.

[0259] In one embodiment, such as Figure 20 As shown, a device for detecting the contact force of an instrument is provided, comprising: a parameter acquisition module 2002, a detection method determination module 2004, an external torque determination module 2006, and a contact force determination module 2008, wherein:

[0260] The parameter acquisition module 2002 is used to acquire the joint parameters corresponding to each joint of the target instrument in the current cycle; the joint parameters include the angle value and velocity value of the joint.

[0261] The detection method determination module 2004 is used to determine the target detection method from a variety of preset detection methods;

[0262] The external torque determination module 2006 is used to determine the external torque of the target instrument in the current cycle based on the joint parameters corresponding to each joint and the target detection method.

[0263] The contact force determination module 2008 is used to determine a value corresponding to the external torque and used to reflect the magnitude of the force based on the mapping relationship between torque and force, and to use the determined value as the contact force value of the end of the target instrument in the current cycle.

[0264] In one embodiment, the joint includes 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 is driven by a wire drive. The external torque determination module 2006 is used to determine the theoretical torque corresponding to each of the first joints based on the joint parameters corresponding to each of the first joints, using a dynamic model corresponding to the non-wire drive method. It also determines the theoretical torque corresponding to each of the second joints based on the joint parameters corresponding to each of the second joints, using a dynamic model corresponding to the wire drive method. Furthermore, it acquires the current value corresponding to each joint and / or acquires the sensing data collected by the sensors corresponding to each joint. Finally, it determines the actual torque corresponding to each joint based on the mapping relationship between current and torque, and the current value corresponding to each joint. Alternatively, for each first joint, first sensing data collected by the sensor corresponding to the first joint is acquired, and the actual torque of the first joint is determined based on the first sensing data. The current value corresponding to each second joint is acquired, and the actual torque corresponding to each second joint is determined based on the mapping relationship between current and torque. For each joint, the difference between the theoretical torque and the actual torque corresponding to the joint is taken as the sub-external torque corresponding to the joint. The sub-external torques corresponding to each joint are fused to obtain the external torque of the target device in the current cycle.

[0265] In one embodiment, the external torque determination module 2006 is used to acquire the current value corresponding to each joint, and determine the actual torque corresponding to each joint based on the mapping relationship between current and torque. Based on the joint parameters corresponding to each joint and the actual torque corresponding to each joint, and through an impulse model, the external torque of the target device in the current cycle is determined.

[0266] In one embodiment, the external torque determination module 2006 is used to determine the impulse value corresponding to each joint based on the joint parameters corresponding to each joint. It determines at least one preceding cycle prior to the current cycle, determines the external torque corresponding to each preceding cycle, and determines the external torque of the target device in the current cycle based on the external torques corresponding to the preceding cycles, the actual torques corresponding to each joint in the current cycle, and the impulse values ​​corresponding to each joint, using an impulse model.

[0267] In one embodiment, the parameter acquisition module 2002 is used to acquire the contact force value in the previous cycle, and take the cycle adjacent to and preceding the previous cycle as the target preceding cycle, and acquire the target parameters corresponding to each joint of the target device in the target preceding cycle, including joint parameters. Based on the target parameters corresponding to each joint of the target device in the target preceding cycle and the contact force value in the previous cycle, the measurement error is determined. Measurement data corresponding to each joint in the current cycle, fed back from the motor in the target device, is acquired. The measurement data is corrected based on the measurement error to obtain the target parameters corresponding to each joint of the target device in the current cycle, and the joint parameters are extracted from the target parameters corresponding to each joint of the target device in the current cycle.

[0268] In one embodiment, the device further includes a target torque determination module, which is used to determine the value of the force to be compensated at the end of the target instrument based on the value of the contact force corresponding to the current cycle, provided that the number of uses of the target instrument is a threshold number of uses. Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated within the current cycle is determined through the mapping relationship between translational degrees of freedom and pose degrees of freedom. The theoretical torque corresponding to each joint within the current cycle is determined. Based on the theoretical torque and compensation torque corresponding to each joint within the current cycle, a target torque is determined, which is used to propel the target instrument to the desired position.

[0269] In one embodiment, the target torque determination module is used to acquire the pose torque generated by each first joint during motor-driven movement, provided that the number of times the target device is used is a threshold number of uses. It acquires the theoretical torque corresponding to each joint in the current cycle and determines the translational torque based on the contact force value and the theoretical torque corresponding to each joint in the current cycle. Based on the pose torque and translational torque, a target torque is determined, which is used to propel the target device to the desired position.

[0270] In one embodiment, the target torque determination module is used to determine the value of the force to be compensated at the end of the target instrument based on the value of the contact force corresponding to the current cycle. Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated within the current cycle is determined through the mapping relationship of the translational degrees of freedom. The translational torque is determined based on the compensation torque and the actual torques corresponding to each joint within the current cycle.

[0271] In one embodiment, the device further includes a transmitting module for sending an alarm signal to a display device when the value of the contact force is greater than or equal to a threshold.

[0272] Each module in the aforementioned instrument contact force detection device can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device as software, so that the processor can call and execute the operations corresponding to each module.

[0273] In one embodiment, a computer device is provided, which may be a server, and its internal structure diagram may be as follows: Figure 21 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and databases. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communicating with external terminals via a network connection. When the computer program is executed by the processor, it implements a method for detecting the contact force of an instrument.

[0274] Those skilled in the art will understand that Figure 21 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0275] In one embodiment, a computer device is also provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps in the above method embodiments.

[0276] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon that, when executed by a processor, implements the steps in the above method embodiments.

[0277] In one embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above method embodiments.

[0278] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data shall comply with the relevant laws, regulations and standards of the relevant countries and regions.

[0279] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM). The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0280] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0281] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A method for detecting the contact force of an instrument, characterized in that, The method includes: Obtain the joint parameters corresponding to each joint of the target instrument within the current cycle; the joint parameters include the angle value and velocity value of the joint; Based on the needs of the current cycle, the target detection method is determined from a variety of preset detection methods; Based on the joint parameters corresponding to each joint and the target detection method, the external torque of the target device in the current cycle is determined; Based on the mapping relationship between torque and force, a value corresponding to the external torque and used to reflect the magnitude of the force is determined, and the determined value is used as the contact force value of the end of the target instrument in the current cycle; The joint includes a first joint near the end of the target device and a second joint not near the end of the target device. The second joint is driven by a wire drive and is a main joint directly driven by a motor. Determining the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method includes: Based on the joint parameters corresponding to each first joint, the theoretical torque corresponding to each first joint is determined by the dynamic model corresponding to the non-wire transmission method. Based on the joint parameters corresponding to each second joint, the theoretical torque corresponding to each second joint is determined by the dynamic model corresponding to the wire transmission method. For each first joint, first sensing data is acquired by sensors installed at relevant positions of the first joint. Based on the first sensing data, the change in the reflection center wavelength of the grating in the sensor is acquired. Based on the change, the stress corresponding to the first joint is determined. Based on the stress, the actual torque of the first joint is determined. The current value corresponding to each second joint is acquired. Based on the mapping relationship between current and torque, the actual torque corresponding to each second joint is determined. For each joint, the difference between the theoretical torque and the actual torque corresponding to the joint is taken as the sub-external torque corresponding to the joint; by fusing the sub-external torques corresponding to each joint, the external torque of the target device in the current cycle is obtained.

2. The method according to claim 1, characterized in that, The step of determining the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method includes: Obtain the current value corresponding to each joint; based on the mapping relationship between current and torque, and the current value corresponding to each joint, determine the actual torque corresponding to each joint. For each joint, the difference between the theoretical torque and the actual torque corresponding to the joint is taken as the sub-external torque corresponding to the joint; by fusing the sub-external torques corresponding to each joint, the external torque of the target device in the current cycle is obtained.

3. The method according to claim 1, characterized in that, The step of determining the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method includes: Obtain the current value corresponding to each joint, and determine the actual torque corresponding to each joint based on the mapping relationship between current and torque; Based on the joint parameters and actual torques corresponding to each joint, the external torque of the target device in the current cycle is determined using an impulse model.

4. The method according to claim 3, characterized in that, The step of determining the external torque of the target device in the current cycle based on the joint parameters and actual torques corresponding to each joint, and using an impulse model, includes: Based on the joint parameters corresponding to each joint, determine the impulse value corresponding to each joint. At least one preceding cycle is determined, and the external torque corresponding to each preceding cycle is determined. Based on the external torque corresponding to each preceding cycle, the actual torque corresponding to each joint in the current cycle, and the impulse value corresponding to each joint, the external torque of the target device in the current cycle is determined by the impulse model.

5. The method according to claim 1, characterized in that, The step of obtaining the joint parameters corresponding to each joint of the target device in the current cycle includes: The contact force value in the previous cycle is obtained, and the cycle adjacent to the previous cycle and located before the previous cycle is taken as the target preceding cycle. The target parameters corresponding to each joint of the target device in the target preceding cycle are obtained respectively, and the target parameters include joint parameters. The measurement error is determined based on the target parameters corresponding to each joint of the target instrument in the preceding cycle and the contact force value in the previous cycle. Acquire measurement data corresponding to each joint in the current cycle from the motor feedback in the target instrument; The measurement data is corrected based on the measurement error to obtain the target parameters corresponding to each joint of the target device in the current cycle, and the joint parameters are extracted from the target parameters corresponding to each joint of the target device in the current cycle.

6. The method according to claim 1, characterized in that, The method further includes: When the number of times the target device is used is within a threshold, the value of the force to be compensated at the end of the target device is determined based on the value of the contact force corresponding to the current cycle. Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated in the current cycle is determined through the mapping relationship between the translational degrees of freedom and the pose degrees of freedom. Determine the theoretical torque corresponding to each joint in the current cycle; Based on the theoretical torque and the compensation torque corresponding to each joint in the current cycle, a target torque is determined, which is used to enable the target instrument to reach the desired position.

7. The method according to claim 2, characterized in that, The method further includes: When the number of times the target device is used is a threshold number of times it is used, the positional torque generated by each first joint when it moves under the motor drive is obtained. Obtain the theoretical torque corresponding to each joint in the current cycle, and determine the translation torque based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint in the current cycle. Based on the pose torque and the translation torque, a target torque is determined, which is used to propel the target instrument to the desired position.

8. The method according to claim 7, characterized in that, The determination of the translational moment based on the contact force value corresponding to the current cycle and the theoretical torque corresponding to each joint within the current cycle includes: Based on the contact force value corresponding to the current cycle, determine the value of the force to be compensated at the end of the target device; Based on the value of the force to be compensated, the compensation torque corresponding to the force to be compensated in the current cycle is determined through the mapping relationship of translational degrees of freedom. The translation torque is determined based on the compensation torque and the actual torque corresponding to each joint in the current cycle.

9. The method according to any one of claims 1 to 8, characterized in that, The method further includes: If the value of the contact force is greater than or equal to the threshold, an alarm signal is sent to the display device.

10. A device for detecting the contact force of an instrument, characterized in that, The device includes: The parameter acquisition module is used to acquire the joint parameters corresponding to each joint of the target instrument in the current cycle; the joint parameters include the angle value and velocity value of the joint. The detection method determination module is used to determine the target detection method from a variety of preset detection methods based on the needs of the current cycle. The external torque determination module is used to determine the external torque of the target device in the current cycle based on the joint parameters corresponding to each joint and the target detection method. The contact force determination module is used to determine a value corresponding to the external torque and used to reflect the magnitude of the force based on the mapping relationship between torque and force, and to use the determined value as the contact force value of the end of the target instrument in the current cycle; The joint includes a first joint near the end of the target device and a second joint not near the end of the target device. The second joint is driven by a wire drive and is a main joint directly driven by a motor. The external torque determination module is further used to determine the theoretical torque corresponding to each first joint based on the joint parameters corresponding to each first joint and through a dynamic model corresponding to the non-wire drive method; to determine the theoretical torque corresponding to each second joint based on the joint parameters corresponding to each second joint and through a dynamic model corresponding to the wire drive method; and for each first joint, to obtain the torque of the transmission installed at the relevant position of the targeted first joint. The sensor collects first sensing data, and based on the first sensing data, obtains the change in the reflection center wavelength of the grating in the sensor. Based on the change, the stress corresponding to the first joint is determined, and based on the stress, the actual torque of the first joint is determined. The current value corresponding to each second joint is obtained, and based on the mapping relationship between current and torque, the actual torque corresponding to each second joint is determined. For each joint, the difference between the theoretical torque and the actual torque corresponding to the joint is taken as the sub-external torque corresponding to the joint. The sub-external torques corresponding to each joint are fused to obtain the external torque of the target device in the current cycle.

11. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 9.

Images