Control method, device, remote control system and storage medium of apparatus

Abstract

The present application relates to a control method, device, teleoperation system, storage medium and computer program product for an instrument. The method comprises: obtaining a command signal sent by an operating workbench, wherein the command signal carries the initial angles of each joint of the target instrument. Determine the command torque for the current time according to the initial angles of each joint of the target instrument, and control the movement of the target instrument according to the command torque. The contact force generated after the movement of the target instrument is theoretically calculated and actually detected to obtain the theoretical contact force and the actual contact force. If the difference between the theoretical contact force and the actual contact force is not within a threshold range, a new command torque is determined based on the initial angle and the difference. The new command torque is used as the next command torque, and the target instrument movement is controlled according to the command torque until the difference is within the threshold range and stops, so as to realize the dynamic balance control of the target instrument, so that the instrument reaches the most uniform control state, and ensures that the instrument can be stably controlled.

Description

Technical Field

[0001] The present application relates to the field of equipment control technology, and in particular to a control method, device, remote operation system, storage medium and computer program product of an apparatus. Background Art

[0002] With the development of equipment control technology, a technology has emerged that uses instruments to repair abnormal parts of target objects. In this technology, the operator remotely controls the robotic arm so that the instrument installed on the robotic arm reaches the abnormal part and performs the corresponding abnormal repair operation. When the instrument performs each repair step, the wire transmission method of the instrument joint is used to realize the movement of the instrument, thereby completing the corresponding repair step. In order to avoid errors in the movement of the instrument, the instrument needs to be controlled accordingly.

[0003] In the prior art, the tension of the wire on each joint is observed in real time, and the control of the device is achieved according to the minimum tension. However, errors are prone to occur only by the tension on the joint, and the problem of being unable to stably control the device occurs. Summary of the invention

[0004] Based on this, it is necessary to provide a control method, device, remote operation system, computer-readable storage medium and computer program product that can stably control the instrument in order to solve the above-mentioned technical problems.

[0005] In a first aspect, the present application provides a method for controlling an apparatus. The method comprises:

[0006] Acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device;

[0007] Determining the current command torque according to the initial angles of each joint of the target device, and controlling the movement of the target device according to the command torque;

[0008] The contact force generated by the movement of the target device is theoretically calculated and actually tested to obtain a theoretical contact force and an actual contact force;

[0009] If the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference;

[0010] The new command torque is used as the next command torque, and the process of controlling the movement of the target device according to the command torque is returned to continue until the difference is within a threshold range and stops, so as to achieve dynamic balance control of the target device.

[0011] In a second aspect, the present application also provides a control device for an apparatus. The device comprises:

[0012] An acquisition module, used to acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device;

[0013] A control module, used to determine the current command torque according to the initial angles of each joint of the target device, and control the movement of the target device according to the command torque;

[0014] An acquisition module, used for theoretically calculating and actually detecting the contact force generated by the target device after the movement, to obtain a theoretical contact force and an actual contact force;

[0015] a determination module, configured to determine a new command torque according to the initial angle and the difference if the difference between the theoretical contact force and the actual contact force is not within a threshold range;

[0016] The return module is used to use the new command torque as the next command torque, and return to the control of the target device movement according to the command torque to continue execution until the difference is within a threshold range and stops, so as to achieve dynamic balance control of the target device.

[0017] In a third aspect, the present application also provides a remote operation system. The remote operation system includes:

[0018] An operating table, used to generate a command signal, wherein the command signal carries the initial angle of each joint of the target device;

[0019] A controller is used to obtain a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device; determine the command torque at that time according to the initial angles of each joint of the target device, and control the movement of the target device according to the command torque; perform theoretical calculation and actual detection on the contact force generated after the movement of the target device, respectively, to obtain a theoretical contact force and an actual contact force; if the difference between the theoretical contact force and the actual contact force is not within a threshold range, determine a new command torque according to the initial angle and the difference; use the new command torque as the next command torque, return to the control of the movement of the target device according to the command torque and continue to execute until the difference is within the threshold range and stop, so as to achieve dynamic balance control of the target device.

[0020] In a fourth aspect, the present application further provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0021] Acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device;

[0022] Determining the current command torque according to the initial angles of each joint of the target device, and controlling the movement of the target device according to the command torque;

[0023] The contact force generated by the movement of the target device is theoretically calculated and actually tested to obtain a theoretical contact force and an actual contact force;

[0024] If the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference;

[0025] The new command torque is used as the next command torque, and the process of controlling the movement of the target device according to the command torque is returned to continue until the difference is within a threshold range and stops, so as to achieve dynamic balance control of the target device.

[0026] In a fifth aspect, the present application further provides a computer program product. The computer program product includes a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0027] Acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device;

[0028] Determining the current command torque according to the initial angles of each joint of the target device, and controlling the movement of the target device according to the command torque;

[0029] The contact force generated by the movement of the target device is theoretically calculated and actually tested to obtain a theoretical contact force and an actual contact force;

[0030] If the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference;

[0031] The new command torque is used as the next command torque, and the process of controlling the movement of the target device according to the command torque is returned to continue until the difference is within a threshold range and stops, so as to achieve dynamic balance control of the target device.

[0032] The control method, device, teleoperation system, storage medium and computer program product of the above-mentioned instrument obtain the command signal sent by the operating workbench, and the command signal carries the initial angle of each joint of the target instrument; the command torque of the current time is determined according to the initial angle of each joint of the target instrument, and the target instrument movement is controlled according to the command torque. In this way, by theoretically calculating and actually detecting the contact force generated after the target instrument moves, the theoretical contact force and the actual contact force can be obtained in real time and accurately. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined according to the initial angle and the difference. That is, the difference is used to evaluate whether the control of the target instrument at the current time reaches dynamic balance. If the difference is not within the threshold range, it means that dynamic balance has not been reached. At this time, the new command torque is used as the next command torque, and the target instrument movement is controlled according to the command torque and continues to be executed until the difference is within the threshold range to stop, so as to realize the dynamic balance control of the target instrument, so that the instrument reaches the most uniform control state, and ensures that the instrument can be stably controlled. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 A diagram showing an application environment of a control method for an apparatus in an embodiment;

[0034] Figure 2 A schematic flow chart of a control method of an apparatus in one embodiment;

[0035] Figure 3 A schematic diagram of the structure of an operating workbench in one embodiment;

[0036] Figure 4 is a schematic diagram of the structure of a target device in one embodiment;

[0037] Figure 5 A schematic diagram of the structure of a sensor in one embodiment;

[0038] Figure 6 A schematic diagram of sensor installation in one embodiment;

[0039] Figure 7 A schematic flow chart of a step of determining a theoretical contact force in one embodiment;

[0040] Figure 8 A schematic diagram of a flow chart of a step of determining a new command torque in one embodiment;

[0041] Fig. 9 A schematic diagram of a flow chart of a step of determining a setting deviation external force in an embodiment;

[0042] Fig.10 A schematic flow chart of a step of determining a desired deviation angle of a joint in one embodiment;

[0043] Fig.11 A schematic flow chart of a step of determining a desired deviation angle of a joint in another embodiment;

[0044] Fig.12 A schematic diagram of a flow chart of a step of determining an initial desired deviation angle of a joint in one embodiment;

[0045] Fig.13 A schematic diagram of a flow chart of a step of determining a new command torque in one embodiment;

[0046] Fig.14 A schematic diagram of a T-shaped motion trajectory in an embodiment;

[0047] Fig.15 A schematic diagram of a flow chart of a step of determining a sub-command torque in one embodiment;

[0048] Fig.16 A schematic diagram of a flow chart of a step of determining a new command torque in one embodiment;

[0049] Fig.17 A schematic diagram of a flow chart of a step of determining a command torque in one embodiment;

[0050] Fig.18 is a schematic diagram of a force control process in one embodiment;

[0051] Fig.19 is a structural block diagram of a control device of an apparatus in one embodiment;

[0052] Fig. 20 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0054] The control method of the device provided in the embodiment of the present application can be applied to Figure 1In the application environment shown. Among them, the application scenario includes an operating workbench 100 for operators to operate, a repair work trolley 200 for a robot arm to repair a target object, an image trolley 300 for displaying images, a tool trolley 400 for placing repair tools, and an auxiliary component 500 for placing auxiliary tools. Among them, the operating workbench 100 is provided with a main operator. In one embodiment, the operator operates the target object through the main operator on the operating workbench 100. The repair work trolley 200 includes at least two robot arms 201, and the instruments and endoscopes are mounted on the robot arms 201 respectively. In one embodiment, the main operator and the robot arms and instruments form a master-slave relationship. For example, during the operation, the operator controls the movement of the robot arms and instruments by controlling the main operator. The repair work trolley 200 communicates with the computer device 600 through a network, and the repair work trolley 200 communicates with the operating workbench 100 through a network ( Figure 1 The repair work trolley 200 communicates with the image trolley 300 via a network ( Figure 1 (not shown), the operating workbench 100 communicates with the computer device 600 through the network. Among them, the computer device 600 can be a server or a terminal. It should be noted that the computer device 600 involved here can be a part of the remote operation system. Specifically, the computer device 600 can be regarded as a controller in the remote operation system. The terminal can be but not limited to various personal computers, laptops, smart phones, tablets, Internet of Things devices and portable wearable devices. The Internet of Things devices can be smart speakers, smart TVs, smart air conditioners, smart car-mounted devices, etc. Portable wearable devices can be smart watches, smart bracelets, head-mounted devices, etc. The server can be implemented as an independent server or a server cluster consisting of multiple servers.

[0055] The data storage system can store the data that the server needs to process. The data storage system can be integrated on the server or placed on the cloud or other network servers.

[0056] In some embodiments, the computer device 600 obtains the command signal sent by the operating workbench 100, and the command signal carries the initial angle of each joint of the target device. The computer device 600 determines the command torque of the current time according to the initial angle of each joint of the target device, and controls the movement of the target device according to the command torque. The contact force generated after the movement of the target device is theoretically calculated and actually detected to obtain the theoretical contact force and the actual contact force. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, the computer device 600 determines a new command torque based on the initial angle and the difference. The computer device 600 uses the new command torque as the next command torque, returns to control the movement of the target device according to the command torque and continues to execute until the difference is within the threshold range to stop, so as to achieve dynamic balance control of the target device.

[0057] In one embodiment, Figure 2 As shown, a control method of an apparatus is provided, and the method is applied to Figure 1 The computer device 600 in the embodiment is taken as an example to illustrate, and the following steps are included:

[0058] Step S202, obtaining a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device.

[0059] Among them, Figure 3 As shown, it is a schematic diagram of the structure of an operating workbench in one embodiment. The operating workbench includes: an adjustment component 110, a manipulation arm 120, a trolley component 130, and an image component 140 (i.e., a display screen). The two manipulation arms 120 detect the hand motion information of the operator through the control handles at their ends as motion control input; the trolley part 130 is a basic bracket for installing other components, and the control trolley has movable casters, which can be moved or fixed as needed; a foot switch is installed on the trolley component 130 to detect the switch control signal sent by the operator; the adjustment component 110 can electrically adjust the position of the manipulation arm, the image component, the operator's armrest and other devices, that is, the human-machine parameter adjustment function. The image component 140 can provide the operator with a stereoscopic image detected from the image system, and provide the operator with reliable image information for abnormal repair operations. During the abnormal repair operation, the operator sitting on the operating workbench is located outside the disinfection area, and the operator controls the target instrument and laparoscope by operating the control handle at the end of the manipulation arm. The operator observes the intracavitary images transmitted back by the image component, and uses both hands to control the movement of the repair work trolley robotic arm and target instruments to complete various operations, thereby performing abnormal repair operations. At the same time, the operator can control some actions through the foot switch, such as completing electroresection, electrocoagulation and other related operation inputs through the foot switch.

[0060] The command signal is used to change the motion state of the target device, that is, to change the motion state of the target device from a static state to a non-static state. The command signal can be regarded as a kinematic command. The initial angle is the angle specified by the operating table, which is used for the subsequent calculation of the command torque.

[0061] Optionally, the computer device acquires the command signal sent by the operating workbench in real time.

[0062] Exemplarily, the operating workbench sends a command signal to the computer device, and the computer device obtains the sending command in real time. At the same time, the operating workbench synchronously sends a command signal to the repair work trolley to trigger the target instrument on the mechanical arm in the repair work trolley.

[0063] Step S204, determining the current command torque according to the initial angles of the joints of the target device, and controlling the movement of the target device according to the command torque.

[0064] Among them, the current time refers to the current moment, and the command torque is used to control the movement of the target device, that is, to control the target device to move to a specified area. The current command torque refers to the torque when controlling the target device at that time.

[0065] Optionally, when this is the first time, the computer device directly determines the command torque of this time according to the initial angles of each joint of the target device. When this is not the first time, the computer device determines the command torque of this time according to the difference between the initial angles of each joint of the target device and the contact force of the target device in the previous time.

[0066] It should be noted that when the target device performs an abnormal repair operation, a force will be generated between the end of the target device and the abnormal part due to contact, and this force is used as the contact force. The contact force can be obtained through theoretical calculation or actual measurement. Because in the actual abnormal repair operation, there will be interference forces that interfere with the movement of the target device, such as friction. Therefore, in order to overcome the interference force, there is a deviation between the actual contact force actually generated between the end of the target device and the abnormal part and the theoretical contact force obtained by theoretical calculation, that is, the theoretical contact force obtained based on theoretical calculation is not equal to the actual contact force obtained by actual measurement. The difference in contact force here refers to the difference between the actual contact force based on actual measurement processing and the theoretical contact force obtained based on theoretical calculation. This difference is a force and can be regarded as a deviation force.

[0067] Step S206, theoretical calculation and actual detection are performed on the contact force generated by the movement of the target device to obtain a theoretical contact force and an actual contact force.

[0068] The theoretical calculation may be a calculation based on a dynamic model or a calculation based on an impulse model, without any specific limitation.

[0069] Optionally, for the contact force generated after the target device moves, the computer device obtains the joint parameters of each joint collected by the target device encoder, and the computer device performs theoretical calculations on the joint parameters based on the dynamic model to obtain the theoretical contact force. Alternatively, the computer device performs theoretical calculations on the joint parameters based on the impulse model to obtain the theoretical contact force. The joint parameters include the actual angle and actual angular velocity of the joint.

[0070] The computer device obtains the actual contact force collected by the force sensor. The force sensor is used to detect the contact force generated between the end of the instrument and the abnormal part. The force sensor can be a resistance strain gauge pressure sensor, a semiconductor strain gauge pressure sensor, etc. without specific limitation.

[0071] The actual angle and actual angular velocity mentioned above are the angle and angular velocity of each joint after the command torque controls the movement of the target device.

[0072] Exemplarily, for the contact force generated after the target device moves, the computer device obtains the joint parameters of each joint collected by the encoder of the target device. For each joint, the computer device obtains the dynamic model corresponding to the wire drive mode, and the dynamic model corresponding to the wire drive mode, performs theoretical calculations on the joint parameters of the targeted joint, and determines the joint torque of the targeted joint. The computer device determines the theoretical contact force based on the joint torque of each joint. The force sensor deployed at the end of the target device collects the actual contact force, and the computer device obtains the actual contact force sent by the force sensor.

[0073] Wherein, the driving mode of the joint is a wire-driven driving mode. The joint can be a wire-driven mode, or a non-wire-driven mode directly driven by a motor. If the joint is a non-wire-driven mode, the joint torque of the joint can be obtained by calculation according to the basic dynamics model. If the joint is a wire-driven mode, the joint torque of the joint can be obtained according to the dynamics model corresponding to the wire-driven driving mode. Wherein, the dynamics model corresponding to the wire-driven driving mode is a model obtained by improving the basic dynamics model based on the wire transmission principle. In the present application, the joints involved are wire-driven joints. In one embodiment, Figure 4The above is a schematic diagram of the structure of a target instrument of an embodiment. The target instrument is a front-end actuator including a wire transmission unit for driving a joint, wherein the wire transmission unit is used to drive the joint by wire. The wire transmission unit includes structures such as a guide wire, a transmission wheel, and a driving wheel. The target instrument has a force sensing unit on the guide wire. During the abnormal repair operation, the operator controls the terminal position and posture of the target instrument through master-slave remote operation under the guidance of the endoscopic image. The terminal position of the target instrument includes the translational movement of the target instrument along the three directions of X, Y, and Z, and the posture includes the pitch, yaw and rotation movement of the terminal of the instrument, which correspondingly involve the pitch joint, the rotation joint and the yaw joint. The control of the target instrument can also be regarded as controlling the joints of the target instrument to achieve the movement of the target instrument.

[0074] Alternatively, for the contact force generated after the target device moves, the computer device obtains the joint parameters and joint current values ​​of each joint collected by the target device encoder. Based on the impulse model, the computer device performs theoretical calculations on the joint parameters and current values ​​of each joint to determine the theoretical contact force. The force sensor deployed at the end of the target device collects the actual contact force, and the computer device obtains the actual contact force sent by the force sensor.

[0075] Step S208: If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined according to the initial angle and the difference.

[0076] As mentioned above, the smaller the difference, the closer the theoretical contact force is to the actual contact force. When the difference is within the threshold range, it can be considered that the actual contact force is equal to the theoretical contact force. When the difference is not within the threshold range, the actual contact force is not equal to the theoretical contact force.

[0077] Optionally, the computer device determines the difference between the theoretical contact force and the actual contact force. If the difference is within a threshold range, the computer device determines that the target device achieves dynamic balance control. If the difference is not within the threshold range, the computer device determines a new command torque based on the initial angle and the difference.

[0078] The difference may be obtained by subtracting the actual contact force from the theoretical contact force, or may be obtained by subtracting the theoretical contact force from the actual contact force, without any specific limitation.

[0079] After obtaining the difference, illustratively, if the difference is within the threshold range, the computer device determines that the target device achieves dynamic balance control. If the difference is not within the threshold range, the computer device determines the set deviation external force of each joint based on the difference. The computer device determines the expected deviation angle of each joint based on the set deviation external force of each joint. The computer device determines a new command torque based on the expected deviation angle and the initial angle. Among them, the expected deviation angle reflects the deviation angle of the joint caused by the set deviation external force.

[0080] It should be noted that the set deviation external force is a deviation external force within the deviation external force threshold, wherein the deviation external force of the joint reflects the force change of the guide wire that controls the movement of the joint. The greater the deviation external force of the joint, the greater the force change of the guide wire that controls the movement of the joint, and the easier it is for the joint to be damaged. Since the set deviation external force is a deviation external force within the deviation external force threshold, the problem of joint damage can be effectively avoided by setting the deviation external force of each joint, ensuring that an effective new command torque can be obtained later.

[0081] Step S210, taking the new command torque as the next command torque, returning to control the movement of the target device according to the command torque and continuing to execute until the difference is within the threshold range, so as to achieve dynamic balance control of the target device.

[0082] Optionally, when entering the next iteration, the computer device uses the new command torque as the next command torque, returns to control the movement of the target device according to the command torque and continues to execute until the difference is within a threshold range and stops the iteration to achieve dynamic balance control of the target device.

[0083] It should be noted that once the target device reaches dynamic balance control, it means that the target device reaches the most uniform control state. At this time, the service life of the target device can be optimized, that is, the number of times the target device is used can be increased.

[0084] In the control method of the above-mentioned device, by obtaining the command signal sent by the operating workbench, the command signal carries the initial angle of each joint of the target device; the command torque of the current time is determined according to the initial angle of each joint of the target device, and the target device movement is controlled according to the command torque. In this way, by theoretically calculating and actually detecting the contact force generated after the target device moves, the theoretical contact force and the actual contact force can be obtained in real time and accurately. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined according to the initial angle and the difference. That is, the difference is used to evaluate whether the control of the target device at the current time reaches dynamic balance. If the difference is not within the threshold range, it means that dynamic balance has not been reached. At this time, the new command torque is used as the next command torque, and the target device movement is controlled according to the command torque and continues to be executed until the difference is within the threshold range to stop, so as to realize the dynamic balance control of the target device, so that the device reaches the most uniform control state, ensuring that the device can be stably controlled. In addition, when the target device reaches dynamic balance, the service life of the device can be optimized.

[0085] In some embodiments, the step of calculating the theoretical contact force includes: determining a target detection method. Obtaining joint parameters corresponding to each joint of the target device; the joint parameters include the actual angle and actual angular velocity of the joint. According to the joint parameters corresponding to each joint, the target detection method is used to calculate the theoretical contact force at the end of the target device.

[0086] The target detection method is used to determine the theoretical contact force of the end of the target instrument. The target detection method can be a first detection method using the wire transmission principle, a second detection method using a stress sensor, or a third detection method using joint impulse, which is not specifically limited.

[0087] Optionally, the computer device selects a detection method from a plurality of detection methods as a target detection method. The computer device obtains joint parameters acquired by an encoder of the target device. The computer device determines the external torque at that time using the target detection method according to the joint parameters corresponding to each joint, and determines the theoretical contact force at the end of the target device based on the external torque.

[0088] The external torque is a torque that can cause the target device to rotate. The external force here refers to the contact force. The external torque includes the sub-external torques corresponding to each joint. In mathematical expression, it can be regarded as a vector, which includes the values ​​of the sub-torques corresponding to each joint.

[0089] Exemplarily, if the computer device uses the first detection method as the target detection method, the computer device obtains the dynamic model corresponding to the wire transmission method. For each joint, the computer device determines the theoretical torque of the joint according to the joint parameters of the joint through the dynamic model corresponding to the wire transmission method. The computer device obtains the current value of each joint, and determines the actual torque of each joint according to the current value of each joint and the mapping relationship between the current and the torque. For each joint, the computer device determines the sub-external torque of the joint according to the difference between the theoretical torque and the actual torque of the joint. The computer device determines the external torque of the time according to the sub-external torque of each joint, and processes the external torque through the inverse matrix of the transposed matrix of the Jacobian matrix to determine the theoretical contact force at the end of the target instrument.

[0090] For example, for each joint i, the computer device determines the theoretical torque τ corresponding to the joint expi and the actual torque τ fdbi After that, the external moment τ corresponding to the joint exti Determined by the following formula:

[0091] τ exti =τ expi -τ fdbi

[0092] After determining the Jacobian matrix J and external moment τ used to determine the contact force ext Then, the theoretical contact force f is determined based on the following formula ext :

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

[0094] As mentioned above, the joints involved in this application are wire-driven. Of course, in actual applications, there may be joints that are not wire-driven according to needs. At this time, if the first detection method is used to calculate the theoretical contact force, for each joint that is not wire-driven, the basic dynamic model can be used to calculate the joint parameters of the joint to determine the theoretical torque of the joint; similarly, the actual torque of the joint is determined based on the current value of the joint and the mapping relationship between the current and the torque. Based on this, for each joint of the target instrument (including wire-driven joints and non-wire-driven joints at this time), the sub-external torque of the joint is determined based on the difference between the theoretical torque and the actual torque of the joint.

[0095] If the computer device uses the second detection method as the target detection method, the computer device obtains the dynamic model corresponding to the wire transmission method. For each joint, the computer device determines the theoretical torque of the joint according to the joint parameters of the joint through the dynamic model corresponding to the wire transmission method. The computer device obtains the first sensing data of each joint acquired by the stress sensor of each joint, and determines the actual torque of each joint according to each first sensing data. For each joint, the computer device determines the sub-external torque of the joint according to the difference between the theoretical torque and the actual torque of the joint. The computer device determines the external torque of the time according to the sub-external torque of each joint, and processes the external torque through the inverse matrix of the transposed matrix of the Jacobian matrix to determine the theoretical contact force at the end of the target instrument.

[0096] The stress sensor may be a miniature FBG (Fiber Bragg Grating) sensor. Figure 5 As shown in the figure, it is a schematic diagram of the structure of a sensor in one embodiment. An optical fiber is arranged in the center of the stress sensor, and the optical fiber is wrapped by a nickel-titanium alloy tube. The sensing principle is that once a force acts on the grating, the central wavelength of the reflection will change. For example, a strain sensor with an outer diameter of 2mm and a length of 7mm is used to adapt well to the narrow space of the target device. In order to facilitate the monitoring of the stress of each joint, the following is used: Figure 6 The stress sensor is installed on each joint in the manner shown. Figure 6 As shown, it is a schematic diagram of the installation of a sensor in an embodiment, where the two ends of the guide wire are fixed to the two ends of the sensor so that the sensor is suspended in the air. If the guide wire connecting the joint is subjected to force, the sensor will be compressed or stretched along the axial direction.

[0097] For example, for each joint i, the computer device determines the theoretical torque τ corresponding to the joint expi and the actual torque τ fdbi After that, the external moment τ corresponding to the joint exti Determined by the following formula:

[0098] τ exti =τ expi -τ fdbi

[0099] After determining the Jacobian matrix J and external moment τ used to determine the contact force ext Then, the contact force f is determined based on the following formula ext :

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

[0101] The steps for determining the actual torque of each joint are as follows: for each joint, the computer device determines the change in the reflection center wavelength of the grating corresponding to the joint according to the corresponding stress sensor, and determines the stress of the joint according to the change and the first function. The first function is a function in which the change in the wavelength of the reflection center changes with the stress. The computer device determines the tension of the guide wire where the joint is located according to the stress of the joint. The computer device determines the actual torque of the joint according to the tension of the guide wire where the joint is located and the second function. The second function is a function in which the torque changes with the tension.

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

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

[0104] Among them, Δλ B K is the change in the reflection center wavelength of the grating; σ represents the stress coefficient, which is a constant; σ z is the axial stress of the sensor. For example, the outer diameter of the sensor is 2 mm and the length is 7 mm, based on which, it is convenient for the target device to perform abnormal repair operations in a narrow space. Among them, the change in the reflection center wavelength is positively correlated with the stress.

[0105] The expression of the second function is as follows:

[0106] τ joint =J*f*r

[0107] Among them, τ joint is the actual torque corresponding to the joint. J is the mapping matrix between the motor and the joint torque. f is the pulling force, and r is the arm from the pulling force to the rotating shaft.

[0108] In some embodiments, Figure 7As shown, it is a flow chart of the step of determining the theoretical contact force in one embodiment. For each joint, the computer device determines the actual angle and actual angular velocity fed back by the encoder deployed on the joint as the actual angle and actual angular velocity of the joint. According to the actual angle and actual angular velocity of the joint, the theoretical torque of the joint is determined by the dynamic model corresponding to the wire transmission mode. For each joint, the computer device determines the change of the reflection center wavelength of the grating corresponding to the joint according to the corresponding stress sensor, and determines the stress of the joint according to the change and the first function. Among them, the first function is a function in which the change of the wavelength of the reflection center changes with the stress. The computer device determines the tension of the guide wire where the joint is located according to the stress of the joint. The computer device determines the actual torque of the joint according to the tension of the guide wire where the joint is located and the second function. The second function is a function in which the torque changes with the tension. For each joint, the computer device determines the sub-external torque of the joint according to the difference between the theoretical torque and the actual torque of the joint. The computer device determines the external torque at that time according to the sub-external torque of each joint, and processes the external torque through the inverse matrix of the transposed matrix of the Jacobian matrix to determine the theoretical contact force at the end of the target instrument.

[0109] As mentioned above, the joints involved in this application are wire-driven. Of course, in actual applications, there may be joints that are not wire-driven according to needs. At this time, if the second detection method is used to calculate the theoretical contact force, for each joint that is not wire-driven, the basic dynamics model can be used to calculate the joint parameters of the joint (i.e., the actual angle and actual angular velocity) to determine the theoretical torque of the joint; similarly, the actual torque of the joint is determined based on the current value of the joint and the mapping relationship between the current and the torque. Based on this, for each joint of the target instrument (including wire-driven joints and non-wire-driven joints at this time), the sub-external torque of the joint is determined based on the theoretical torque and actual torque of the joint.

[0110] If the computer device uses the third detection method as the target detection method, the computer device obtains the current value corresponding to each joint, and determines the actual torque corresponding to each joint according to the mapping relationship between current and torque. The computer device obtains an impulse model constructed based on joint impulse, and determines the external torque of the target device in the current cycle through the impulse model according to the joint parameters corresponding to each joint and the actual torque corresponding to each joint.

[0111] In this embodiment, the external torque can be determined directly by target detection through the joint parameters corresponding to each joint, which can effectively avoid the error caused by indirect measurement. According to the mapping relationship between torque and force, the value corresponding to the external torque and used to reflect the force can be quickly and accurately determined, and the determined value is directly used as the theoretical contact force at the end of the target instrument, which effectively avoids the measurement error caused by indirect measurement, thereby improving the accuracy of instrument contact force detection.

[0112] In some embodiments, after the theoretical contact force is determined, if the theoretical contact force is too large, an alarm may be given in a timely manner.

[0113] Specifically, based on the operating workbench and the repair worktrolley, a master-slave operation is performed, that is, the computer device sets the operating workbench as the master end and the repair worktrolley as the slave end, and performs master-slave mapping so that data can be exchanged between the operating workbench and the repair worktrolley. The computer device determines the target detection method. The joint parameters corresponding to each joint of the target instrument are obtained; the joint parameters include the actual angle and actual angular velocity of the joint. According to the joint parameters corresponding to each joint, the theoretical contact force at the end of the target instrument is calculated using the target detection method.

[0114] The computer device determines whether the theoretical contact force exceeds the threshold value (that is, whether it is greater than or equal to the threshold value. When the value of the theoretical contact force is greater than or equal to the threshold value, the computer device feeds back the information that the theoretical contact force exceeds the threshold value to the main end, and sends an alarm signal to the display device. When the value of the theoretical contact force is less than the threshold value, the computer device performs actual detection to determine the actual contact force. At this time, the computer device determines whether the actual contact force exceeds the threshold value (that is, whether it is greater than or equal to the threshold value. When the value of the actual contact force is greater than or equal to the threshold value, the computer device feeds back the information that the actual contact force exceeds the threshold value to the main end, and sends an alarm signal to the display device. When the value of the actual contact force is less than the threshold value, the computer device returns to step S208 to continue execution.

[0115] In this embodiment, by making a real-time judgment on the theoretical contact force, it can be timely reflected whether the contact force applied by the master hand to the target device is too large. If it is too large, an alarm is directly issued. If the theoretical contact force does not exceed the threshold, it means that the force applied by the master hand is theoretically appropriate. In this way, by making a real-time judgment on the actual contact force, it is ensured that the contact force actually applied by the end of the target device on the abnormal part will not cause damage to the abnormal part.

[0116] In some embodiments, Figure 8FIG. 1 is a flow chart of the steps of determining a new command torque in one embodiment. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined based on the initial angle and the difference, including:

[0117] Step S802: If the difference between the theoretical contact force and the actual contact force is not within the threshold range, the difference is subjected to gain processing to obtain the gain deviation external force corresponding to each joint.

[0118] As mentioned earlier, the difference represents the force, that is, the deviation force between the theoretical contact force and the actual contact force. Since the contact force can be regarded as an external force, the deviation force can also be regarded as a deviation external force.

[0119] Gain processing can map the deviation external force at the end of the target device to the deviation external force of each joint, and adjust the deviation external force of each joint to obtain the gain deviation external force.

[0120] Optionally, if the difference between the theoretical contact force and the actual contact force is not within the threshold range, the computer device obtains a gain matrix, and performs gain processing on the difference through the gain matrix to obtain a gain deviation external force corresponding to each joint. The gain matrix includes gain parameters corresponding to each joint.

[0121] Exemplarily, the computer device obtains the gain matrix [λ 1 ,λ 2 ,…λ n-1 ,λ n ] After that, the gain deviation external force corresponding to each joint is determined through the following steps:

[0122] f err_inc =[λ 1 ,λ 2 ,…λ n-1 ,λ n ]*f err

[0123] Among them, λ n Characterize the gain parameter corresponding to the nth joint, f err is the difference, f err_inc is the vector of gain deviation external force, which includes the gain deviation external force corresponding to each joint. Among them, the gain matrix is ​​a diagonal matrix, and the gain parameter corresponding to each joint is a constant value.

[0124] Step S804, detecting the actual angle of each joint.

[0125] Optionally, for each joint, the computer device obtains the actual angle fed back via the encoder of the joint.

[0126] Step S806, for each joint, determine the corresponding deviation external force threshold based on the actual angle of the joint, and determine the set deviation external force of the joint according to the deviation external force threshold of the joint and the value of the gain deviation external force.

[0127] As mentioned above, the deviation external force reflects the change in the force on the guide wire that controls the movement of the joint. Based on this, in order to ensure that the deviation external force of each joint does not exceed the force limit of the guide wire, it is necessary to adjust the gain deviation external force based on the deviation external force threshold to obtain a set deviation external force within the guide wire force limit.

[0128] Furthermore, changes in the actual angle of the joint will result in changes in the deviation external force threshold of the joint.

[0129] Optionally, for each joint, the computer device determines a deviation external force threshold value corresponding to the joint based on the actual angle of the joint. The computer device compares the deviation external force threshold value of the joint with the value of the gain deviation external force to obtain a comparison result, and determines a set deviation external force for the joint based on the comparison result.

[0130] Exemplarily, for each joint, the computer device determines the deviation external force threshold value corresponding to the joint based on the actual angle of the joint. The computer device compares the deviation external force threshold value of the joint with the value of the gain deviation external force to obtain a comparison result, determines the value and direction of the set deviation external force for the joint based on the comparison result, and determines the set deviation external force for the joint based on the value and direction of the set deviation external force for the joint.

[0131] Step S808, performing at least one admittance control process according to the set deviation external force of the joint to obtain the desired deviation angle of the joint.

[0132] Among them, the admittance control is based on the equation of joint torque, joint angle and angular velocity to achieve the control of joint torque, joint angle or angular velocity. Among them, the angular velocity can be regarded as the first-order derivative of the joint angle. Therefore, the admittance control process that reflects the connection between the joint torque and the joint angle and angular velocity can be regarded as a first-order admittance control process.

[0133] Admittance processing can also be based on the equation of joint torque, joint angle, angular velocity and angular acceleration to achieve control of joint torque, or control of joint angle, or control of angular velocity, or control of angular acceleration. Among them, angular acceleration can be regarded as the first-order derivative of angular velocity, that is, the second-order derivative of joint angle. Therefore, the admittance control processing that reflects the connection between the torque of the joint and the joint angle, angular velocity and angular acceleration can be regarded as second-order admittance control processing.

[0134] Optionally, the computing device determines a limiting deviation torque of the targeted joint according to the set external force of the targeted joint, and performs at least one admittance control process on the limiting deviation torque of the targeted joint to obtain a desired deviation angle of the targeted joint.

[0135] Exemplarily, after determining the limiting deviation torque of the targeted joint, the computer device performs first-order admittance processing on the limiting deviation torque of the targeted joint to obtain a desired deviation angle of the targeted joint.

[0136] Alternatively, the computer device performs second-order admittance processing on the limiting deviation torque of the targeted joint to obtain a desired deviation angle of the targeted joint.

[0137] Alternatively, the computer device performs first-order admittance processing on the limit deviation torque of the targeted joint to obtain the first-order admittance processing result of the targeted joint. The computer device performs second-order admittance processing on the limit deviation torque of the targeted joint to obtain the second-order admittance processing result of the targeted joint. The computer device fuses the first-order admittance processing result and the second-order admittance processing result of the targeted joint to determine the expected deviation angle of the targeted joint.

[0138] Step S810, determining a new command torque according to the expected deviation angle, initial angle and actual angle of each joint.

[0139] Among them, the simulated angle and simulated angular velocity are used to control the next movement of the target device. After the target device performs the next movement, the encoders of each joint of the target device will provide real-time feedback of the actual angles of each joint after the movement.

[0140] Optionally, for each joint, the computer device determines the simulation angle of the joint according to the expected deviation angle, initial angle and actual angle of the joint, and determines the simulation angular velocity of the joint at the current moment according to the simulation angle of the joint. The computer device determines a new command torque according to the simulation angle and simulation angular velocity of each joint.

[0141] In the present embodiment, once it is determined that the difference between the theoretical contact force and the actual contact force exceeds the threshold range, the gain deviation external force of each joint is determined in real time according to the difference. According to the actual angle of each joint, the deviation external force threshold value that matches the actual situation of the joint is determined, and based on the deviation external force threshold value of each joint, the gain deviation external force of each joint is adjusted in real time, thereby determining the setting deviation external force. In this way, it is ensured that the setting deviation external force of each joint is a force within the force limit of the guide wire that controls the joint, and damage to the joint is avoided. Based on this, according to the setting deviation external force of each joint and at least one admittance processing, the expected deviation angle is determined in real time, thereby, according to the expected deviation angle, initial angle and actual angle of each joint, the new command torque can be accurately and effectively determined.

[0142] In some embodiments, for each joint, a corresponding deviation external force threshold is determined based on the actual angle of the joint, including: for each joint, a life curve corresponding to the service life of the joint is obtained, and the life curve characterizes the relationship between the deviation external force threshold and the actual angle. For each joint, the deviation external force threshold of the joint is determined according to the actual angle of the joint and the life curve.

[0143] Since the actual angle of the joint will affect the force applied to the guide wire that controls the joint, and the greater the force applied to the guide wire that controls the joint, the easier it is for the joint to be damaged, which will reduce the service life of the targeted joint. Therefore, the change in the actual angle of the joint is related to the service life of the joint. Further, as mentioned above, the actual angle will cause a change in the deviation external force threshold, so the deviation external force threshold that matches the service life of the targeted joint can be determined through the life curve of the targeted joint, thereby ensuring that the set deviation external force of the targeted joint will not cause damage to the targeted joint, that is, determining the optimal set deviation external force, thereby optimizing the service life of the targeted joint, and further optimizing the service life of the target device.

[0144] Optionally, for each joint, the computer device obtains a life curve corresponding to the service life of the joint, and determines a deviation external force threshold corresponding to the actual angle of the joint based on the life curve.

[0145] The life curve also includes the direction of the force corresponding to the deviation external force threshold; for example, the horizontal axis of the life curve represents the actual angle of the joint, and the value of the vertical axis represents the deviation external force threshold. The direction of the vertical axis represents the direction of the force corresponding to the deviation external force threshold. The direction of the force here is relative to the direction of the set deviation external force, that is, if the deviation external force threshold is in the negative direction of the vertical axis, the direction of the force corresponding to the deviation external force threshold is opposite to the direction of the set deviation external force; if the deviation external force threshold is in the positive direction of the vertical axis, the direction of the force corresponding to the deviation external force threshold is the same as the direction of the set deviation external force.

[0146] In this embodiment, for each joint, based on the life curve of the joint, the deviation external force threshold corresponding to the actual angle can be determined in real time and accurately, thereby ensuring that the subsequent adjusted deviation external force obtained based on the deviation external force threshold will not damage the joint, thereby optimizing the service life of each joint.

[0147] In some embodiments, the set deviation external force for the joint is determined based on the values ​​of the deviation external force threshold and the gain deviation external force for the joint, including: comparing the sizes of the deviation external force threshold and the gain deviation external force for the joint, and determining the set deviation external force for the joint based on the smaller value.

[0148] Optionally, the computer device compares the magnitude of the deviation external force threshold value and the gain deviation external force value of the targeted joint to determine the smaller value. If the smaller value is the gain deviation external force value of the targeted joint, the computer device uses the gain deviation external force of the targeted joint as the setting deviation external force of the targeted joint. If the smaller value is the deviation external force threshold value of the targeted joint, the computer device determines the direction of the force corresponding to the deviation external force threshold value of the targeted joint, and determines the setting deviation external force of the targeted joint based on the direction of the force corresponding to the deviation external force threshold value of the targeted joint and the deviation external force threshold value.

[0149] For example, the target device has n+1 joints. For joint i, let the gain deviation external force f of joint i be err_inc [i], the force f corresponding to the deviation external force threshold of joint i life , the set deviation external force f of joint i err_output [i]. The corresponding code is as follows:

[0150]

[0151] The above code uses the for else statement, which can be understood as follows:

[0152] If f err_inc [i]>f lifeThat is, the value of the gain deviation external force of joint i is greater than the deviation external force threshold, and the direction of the gain deviation external force of joint i and the force corresponding to the deviation external force threshold of joint i are the same, then the force corresponding to the deviation external force threshold of joint i is used as the set deviation external force.

[0153] If f err_inc [i]<-f life , that is, the value of the gain deviation external force of joint i is greater than the deviation external force threshold, and the direction of the force corresponding to the gain deviation external force of joint i and the deviation external force threshold of joint i is opposite, then f err_output [i] = -f life , that is, the force corresponding to the deviation external force threshold of joint i is used as the setting deviation external force. err_inc [i]≤f life , or, f err_inc [i]≥-f life , then the gain deviation external force of joint i is taken as the setting deviation external force.

[0154] In this embodiment, the smaller value is determined by comparing the deviation external force threshold value and the gain deviation external force value of the targeted joint. In this way, the set deviation external force determined based on the smaller value will not cause damage to the targeted joint, thereby ensuring the service life of the targeted joint and facilitating the subsequent determination of an effective new command torque.

[0155] In some embodiments, Fig. 9As shown, it is a flow chart of determining the step of setting deviation external force in one embodiment. In this embodiment, it is carried out when it is determined that the difference is not within the threshold range. The implementation subject of this embodiment can be specifically regarded as a life judgment unit in a computer device. The step of determining the setting deviation external force of each joint includes: the life judgment unit in the computer device obtains a gain matrix, and performs gain processing on the difference through the gain matrix to obtain the gain deviation external force corresponding to each joint. Among them, the gain matrix includes the gain parameters corresponding to each joint. For each joint, the life judgment unit in the computer device obtains the life curve corresponding to the service life of the joint, and the life curve characterizes the relationship between the deviation external force threshold and the actual angle. For each joint, the life judgment unit in the computer device determines the deviation external force threshold of the joint according to the actual angle and life curve of the joint. The life curve also includes the direction of the force corresponding to the deviation external force threshold. The life judgment unit in the computer device compares the deviation external force threshold of the joint and the value of the gain deviation external force, and determines the smaller value. If the smaller value is the value of the gain deviation external force of the joint, the life judgment unit in the computer device uses the gain deviation external force of the joint as the setting deviation external force of the joint. If the smaller value is the deviation external force threshold of the joint, the life judgment unit in the computer device determines the direction of the force corresponding to the deviation external force threshold of the joint, and determines the setting deviation external force of the joint according to the direction of the force corresponding to the deviation external force threshold of the joint and the deviation external force threshold.

[0156] In this embodiment, for each joint, according to the life curve of the targeted joint, the deviation external force threshold corresponding to the actual angle can be determined in real time and accurately. By comparing the deviation external force threshold of the targeted joint and the value of the gain deviation external force, a smaller value is determined. In this way, the set deviation external force determined based on the smaller value will not cause damage to the targeted joint, thereby ensuring the service life of the targeted joint, and is also conducive to the subsequent determination of an effective new command torque.

[0157] In some embodiments, Fig.10 FIG. 1 is a flow chart of a step of determining a desired deviation angle of a joint in an embodiment. The step of performing at least one admittance control process according to the set deviation external force of the joint to obtain the desired deviation angle of the joint includes:

[0158] Step S1002, determining the uniform deviation external force of the targeted joint according to the weight of the targeted joint and the set deviation external force.

[0159] Optionally, the computer device determines the weight of the targeted joint, calculates the product of the square value of the set deviation external force of the targeted joint and the weight of the targeted joint, and uses the product as the uniform deviation external force of the targeted joint.

[0160] The weight of each joint can be set according to actual needs, or can be determined by uniform processing, where uniform processing means that the weights of all joints are equal. The following takes the uniform processing to determine the weights of each joint as an example for explanation.

[0161] Exemplarily, the computer device calculates the sum of the squares of the set deviation external forces of each joint to obtain a sum value, and performs square root calculation on the sum value to obtain a square root value, and uses the reciprocal of the square root value as the weight of each joint. For each joint, the computer device uses the product of the square value of the set deviation external force for the joint and the weight as the uniform deviation external force for the joint.

[0162] It should be noted that uniform processing can be understood as uniformly controlling each joint, that is, the weight of each joint is determined by taking the square root of the sum of the squares of all set deviation external forces, that is, the weights of all joints are equal and equal to the square root value.

[0163] For example, if the target device has m joints, namely joint 1 to joint m, the set deviation external force corresponding to joint i is f err_outputm , then the following formula is used to determine the weight f of each joint cart :

[0164]

[0165] At this time, the uniform deviation external force f for joint i is cart[i] :

[0166]

[0167] Step S1004, performing Jacobian matrix transposition processing on the uniform deviation external force of the joint to obtain the limiting deviation torque of the joint.

[0168] Step S1006: Perform at least one admittance control process based on the limit deviation torque of the joint to obtain an initial expected deviation angle of the joint.

[0169] Optionally, the computer device performs first-order admittance processing on the limiting deviation torque of the targeted joint to obtain an initial desired deviation angle of the targeted joint.

[0170] Alternatively, the computer device performs second-order admittance processing on the limiting deviation torque of the targeted joint to obtain an initial desired deviation angle of the targeted joint.

[0171] Alternatively, the computer device performs first-order admittance processing on the limit deviation torque of the targeted joint to obtain the first-order admittance processing result of the targeted joint. The computer device performs second-order admittance processing on the limit deviation torque of the targeted joint to obtain the second-order admittance processing result of the targeted joint. The computer device fuses the first-order admittance processing result and the second-order admittance processing result of the targeted joint to determine the initial expected deviation angle of the targeted joint.

[0172] The first-order admittance processing result mentioned above can be regarded as a first deviation angle corresponding to the first-order admittance, and the second-order admittance processing result can be regarded as a second deviation angle corresponding to the second-order admittance.

[0173] In some embodiments, the step of fusing the first-order admittance processing result and the second-order admittance processing result of the targeted joint to determine the initial expected deviation angle of the targeted joint includes: a computer device directly superimposing the first-order admittance processing result and the second-order admittance processing result of the targeted joint to obtain the expected deviation angle of the targeted joint; or, a computer device obtains weights of the first-order admittance control processing and the second-order admittance control processing, and determines the initial expected deviation angle of the targeted joint through weighted summation of the weights of the first-order admittance control processing, the weights of the second-order admittance control processing, the first-order admittance processing result and the second-order admittance processing result.

[0174] Step S1008, determining an expected deviation angle of the targeted joint according to the initial expected deviation angle of the targeted joint; the expected deviation angle is within a preset threshold range.

[0175] Optionally, the computer device determines whether the initial expected deviation angle of the joint is within a preset threshold range, and determines the expected deviation angle of the joint based on the determination result.

[0176] Exemplarily, if the initial expected deviation angle of the joint is within the preset threshold range, the computer device directly uses the initial expected deviation angle of the joint as the expected deviation angle. If the initial expected deviation angle of the joint is less than the minimum value in the preset threshold range, the computer device uses the minimum value as the expected deviation angle of the joint. If the initial expected deviation angle of the joint is greater than the maximum value in the preset threshold range, the computer device uses the maximum value as the expected deviation angle of the joint.

[0177] It should be noted that the above step of determining whether the initial expected deviation angle is within the preset threshold range is to dynamically limit the initial expected deviation angle, thereby achieving a comparison mechanical limit, that is, ensuring that the expected deviation angle is within the preset threshold range.

[0178] For the above steps S1002 to S1008, in one embodiment, Fig.11As shown, it is a flowchart of the steps of determining the expected deviation angle of the joint in another embodiment. In this embodiment, the implementation subject is a constraint control unit in a computer device.

[0179] Specifically, the constraint control unit in the computer device performs uniform processing according to the set deviation external force of each joint to obtain the uniform deviation external force of each joint. Among them, the steps for determining the uniform deviation external force of each joint are as follows: the constraint control unit in the computer device calculates the sum of the squares of the set deviation external forces of each joint to obtain the sum value, and performs square root calculation on the sum value to obtain the square root value, and uses the inverse of the square root value as the weight of each joint. For each joint, the constraint control unit in the computer device uses the product of the square value of the set deviation external force of the joint and the weight as the uniform deviation external force of the joint.

[0180] The constraint control unit in the computer device performs Jacobian matrix transposition processing on the uniform deviation external force of the targeted joint to obtain the limit deviation torque of the targeted joint. At least one admittance control processing is performed based on the limit deviation torque of the targeted joint to obtain the initial expected deviation angle of the targeted joint. The constraint control unit in the computer device determines whether the initial expected deviation angle of the targeted joint is within a preset threshold range, and determines the expected deviation angle of the targeted joint based on the determination result.

[0181] In this embodiment, uniform control of each joint can be achieved through the weight of the targeted joint and the setting deviation external force, so as to obtain the uniform deviation external force of the targeted joint, and then the corresponding limiting deviation torque can be accurately determined. By performing at least one admittance control process on the limiting deviation torque of the targeted joint, the initial expected deviation angle corresponding to the deviation torque can be accurately determined. According to the initial expected deviation angle of the targeted joint, the expected deviation angle of the targeted joint within the preset threshold range is determined to avoid the expected deviation angle being too large or too small, which is conducive to the subsequent determination of an accurate and effective new command torque.

[0182] In some embodiments, at least one admittance control process is performed based on the limit deviation torque of the joint to obtain the initial desired deviation angle of the joint, including: performing first-order admittance control on the limit deviation torque of the joint through a first-order admittance model to obtain a first deviation angle. Performing second-order admittance control on the limit deviation torque of the joint through a second-order admittance model to obtain a second deviation angle. The first deviation angle and the second deviation angle of the joint are fused to obtain the initial desired deviation angle of the joint.

[0183] As mentioned above, the admittance control process is implemented based on the equation of joint torque, joint angle and angular velocity, and can also be implemented based on the equation of joint torque, joint angle, angular velocity and angular acceleration. Therefore, the first-order admittance model can be regarded as an equation based on joint torque, joint angle and angular velocity. The second-order admittance model can be regarded as an equation based on joint torque, joint angle, angular velocity and angular acceleration.

[0184] Optionally, the computer device inputs the limiting deviation torque of the joint into the first-order admittance model to obtain a first deviation angle. And the limiting deviation torque of the joint is input into the second-order admittance model to obtain a second deviation angle. The computer device superimposes the first deviation angle and the second deviation angle of the joint to obtain an initial desired deviation angle of the joint. Alternatively, the computer device determines the weight of the first-order admittance control processing and the value of the second-order admittance control processing, and determines the initial desired deviation angle of the joint by weighted summation based on the weight of the first-order admittance control processing, the weight of the second-order admittance control processing, the first deviation angle and the second deviation angle.

[0185] For example, Fig.12 The figure is a schematic flow chart of the steps for determining the initial expected deviation angle of a joint in one embodiment.

[0186] The constraint control unit in the computer device performs Jacobian matrix transposition processing on the uniform deviation external force of the joint to obtain the limit deviation torque of the joint. The preset control unit in the computer device determines the weight K1 of the first-order admittance control processing and the value K2 of the second-order admittance control processing. The constraint control unit in the computer device inputs the limit deviation torque of the joint into the first-order admittance model to obtain the first deviation angle. And the limit deviation torque of the joint is input into the second-order admittance model to obtain the second deviation angle. The computer device determines the initial expected deviation angle of the joint by weighted summation based on the weight K1 of the first-order admittance control processing, the weight K2 of the second-order admittance control processing, the first deviation angle and the second deviation angle. Wherein, the sum of K1 and K2 is a constant 1.

[0187] Among them, the first-order admittance model is as follows:

[0188]

[0189] In the above formula, τ is the limiting deviation torque, b is the damping coefficient, and c is the spring coefficient. is the angular velocity of the targeted joint, and θ is the velocity of the targeted joint.

[0190] The second-order admittance model is as follows:

[0191]

[0192] In the above formula, τ is the limiting deviation torque, d is the inertia parameter, b is the damping coefficient, and c is the spring coefficient. is the angular velocity of the targeted joint, θ is the velocity of the targeted joint, is the acceleration of the targeted joint.

[0193] In this embodiment, the first and second deviation angles are obtained respectively by the first-order admittance model and the second-order admittance model. Based on this, the accuracy of the initial desired deviation angle of the joint can be improved by fusing the first and second deviation angles respectively corresponding to the two admittance control processes.

[0194] In some embodiments, Fig.13 FIG. 1 is a flow chart of the steps of determining a new command torque in one embodiment. Determining a new command torque according to the expected deviation angle, initial angle and actual angle of each joint includes:

[0195] Step S1302, determining the expected angle corresponding to each joint according to the expected deviation angle and the initial angle corresponding to each joint.

[0196] Optionally, for each joint, the computer device fuses the desired deviation angle and the initial angle of the joint to determine the desired angle of the joint.

[0197] Exemplarily, for each joint, the computer device superimposes the expected deviation angle and the initial angle of the joint to determine the expected angle of the joint. Alternatively, the computer device determines the weights of the expected deviation angle and the initial angle, and determines the expected angle of the joint by weighted summation based on the weights of the expected deviation angle and the initial angle, the expected deviation angle of the joint, and the initial angle.

[0198] Step S1304, determining the simulation angle of each joint based on the expected angle and the actual angle corresponding to each joint.

[0199] Optionally, for each joint, the computer device calculates the difference between the expected angle and the actual angle of the joint, and uses the difference as the simulated angle of the joint.

[0200] Of course, it is also possible to calculate the difference between the actual angle and the expected angle of the joint, and use the difference as the simulation angle of the joint.

[0201] Step S1306, determining the simulated angular velocity of each joint at the current moment when the joint moves according to the preset motion trajectory according to the simulated angle of each joint.

[0202] Among them, the preset motion trajectory represents the law of joint movement, that is, it represents the movement law of the joint at different times, such as whether the joint is moving at a uniform speed, uniformly accelerated or uniformly decelerated at a certain moment.

[0203] Optionally, the computer device determines a preset motion trajectory from a plurality of polynomial motion trajectories, and determines, based on the simulation angles of the respective joints, a simulated angular velocity of each joint at a current moment when the joint moves along the preset motion trajectory.

[0204] The polynomial motion trajectory can be an n-order polynomial spline difference method (n≥5), an S-shaped trajectory or a T-shaped trajectory. The following takes the T-shaped trajectory as an example. Fig.14 FIG. 1 is a schematic diagram of a T-shaped motion trajectory in an embodiment. Fig.14 The following table shows the change of joint angle (pos) at different times. Figure 1 , the angular velocity (vel) of the joint at different times changes with time Figure 2 , the angular acceleration (acc) of the joint at different times changes with time Figure 3 The T-shaped trajectory involves the uniform acceleration stage, uniform speed stage and uniform deceleration stage. When in the uniform acceleration stage, t 1 The angle q(t 1 ):

[0205]

[0206] In the above formula, q 0 is the angle at time 0. is the angular acceleration during the uniform acceleration phase. c is the end moment of the uniformly accelerated motion phase.

[0207] t 1 Speed ​​corresponding to the moment

[0208]

[0209] t 1 Angular acceleration corresponding to the moment

[0210]

[0211] When in the uniform motion stage, t 2 The angle q(t 2 ):

[0212]

[0213] is time tc The corresponding angle, t c is the starting time of the uniform motion phase, j is the end moment of the uniform motion phase.

[0214] t 2 Speed ​​corresponding to the moment

[0215]

[0216] t 2 Angular acceleration corresponding to the moment

[0217]

[0218] When in the uniform deceleration motion stage, t 3 The angle q(t 3 ):

[0219]

[0220] is time t j The corresponding angle, t j is the starting time of the uniform deceleration motion phase, f is the end moment of the uniform deceleration motion phase. t c The angular acceleration corresponding to the moment.

[0221] t 3 Speed ​​corresponding to the moment

[0222]

[0223] t 3 Angular acceleration corresponding to the moment :

[0224]

[0225] Exemplarily, the computer device determines the simulation function and angular acceleration corresponding to the current moment according to the preset motion trajectory and the current moment, and determines the simulated angular velocity of the joint according to the simulation function, the simulated velocity and angular acceleration of the joint. The simulation function is a function in which the simulation velocity changes with the changes of the simulated angular velocity and angular acceleration.

[0226] Step S1308, determining the sub-command torque corresponding to each joint according to the simulation angle and simulation angular velocity corresponding to each joint.

[0227] Optionally, for each joint, the computer device determines the first target sub-torque corresponding to the joint by at least one of proportional calculation and integral calculation according to the simulation angle corresponding to the joint, and determines the second target sub-torque of the joint by differential calculation according to the simulation angular velocity corresponding to the joint. The computer device fuses the first target sub-torque and the second target sub-torque of each joint to determine the sub-command torque corresponding to each joint.

[0228] Exemplarily, for each joint, the computer device determines the first target sub-torque of the joint by integral calculation according to the simulation angle corresponding to the joint, and determines the second target sub-torque of the joint by differential calculation according to the simulation angular velocity corresponding to the joint. The computer device superimposes the first target sub-torque and the second target sub-torque of each joint to determine the command torque corresponding to each joint.

[0229] Alternatively, for each joint, the computer device determines the first target sub-torque of the joint by proportional calculation according to the simulation angle corresponding to the joint, and determines the second target sub-torque of the joint by differential calculation according to the simulation angular velocity corresponding to the joint. The computer device superimposes the first target sub-torque and the second target sub-torque of each joint to determine the command torque corresponding to each joint.

[0230] Alternatively, for each joint, the computer device determines the first sub-torque of the joint through integral calculation based on the simulation angle corresponding to the joint, and determines the second sub-torque of the joint through proportional calculation based on the simulation angle corresponding to the joint, and superimposes the first sub-torque and second sub-torque of each joint to determine the first target sub-torque corresponding to each joint.

[0231] The computer device determines the second target sub-torque of the joint through differential calculation according to the simulated angular velocity corresponding to the joint. The computer device superimposes the first target sub-torque and the second target sub-torque of each joint to determine the command torque corresponding to each joint.

[0232] Step S1310, determining a new command torque according to the sub-command torques corresponding to the joints.

[0233] Optionally, the computer device combines the sub-command torques corresponding to each joint to form a new command torque.

[0234] In this embodiment, the expected angle corresponding to each joint can be determined in real time through the expected deviation angle and initial angle corresponding to each joint. The computer device can accurately determine the simulation angle of each joint based on the expected angle and actual angle corresponding to each joint. Based on this, the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory is determined based on the simulation angle of each joint. In this way, according to the simulation angle and simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint can be determined in a timely and accurate manner, thereby obtaining an effective and accurate new command torque.

[0235] In some embodiments, Fig.15 FIG. 1 is a flow chart of the step of determining a sub-command torque in one embodiment. According to the simulation angle and simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint is determined, including:

[0236] Step S1502, determining the actual angle and actual angular velocity corresponding to each joint.

[0237] Step S1504: For each joint, an integral calculation is performed based on the simulation angle and the actual angle of the joint to obtain a first sub-torque.

[0238] Optionally, for each joint, the computer device performs an integral calculation based on the simulated angle and the actual angle of the joint to obtain an integral calculation result, and performs a restriction process based on the integral calculation result to obtain a first sub-torque.

[0239] Exemplarily, for each joint, the computer device calculates the difference between the simulated angle and the actual angle of the joint to obtain a first difference, and performs an integral calculation on the first difference to obtain an integral calculation result. The computer device inputs the integral calculation result into a limit function to obtain a first sub-torque. The limit function is used to perform saturation limit on the joint to achieve the optimal effect of force control.

[0240] For example, θ sim and θ are the simulated angle and the actual angle of the joint respectively, the computer device calculates the difference between the simulated angle and the actual angle of the joint, obtains a first difference, and determines the integral calculation result based on the product of the first difference and the position controller stiffness corresponding to the integral calculation. At this time, the computer device obtains the maximum torque of the position loop corresponding to the integral calculation.

[0241] Based on this, the first sub-moment τ is determined by the following formula: i :

[0242] τ i =limit(-τ imax ,Ki*∫(θ sim -θ)dt,τimax )

[0243] Among them, θ sim represents the simulation angle, θ represents the actual angle, Ki is the position controller stiffness corresponding to the integral calculation, τ imax To calculate the maximum torque of the position loop corresponding to the integral, limit(.) is a limiting function.

[0244] Step S1506, performing proportional calculation according to the simulation angle and the actual angle of the joint to obtain a second sub-torque.

[0245] Optionally, for each joint, the computer device performs a proportional calculation based on the simulated angle and the actual angle of the joint to obtain a proportional calculation result, and performs a restriction process based on the proportional calculation result to obtain a second sub-torque.

[0246] Exemplarily, for each joint, the computer device calculates the difference between the simulated angle and the actual angle of the joint to obtain a second difference, and performs proportional calculation on the second difference to obtain a proportional calculation result. The computer device inputs the proportional calculation result into the limit function to obtain a second sub-torque. The limit function is used to perform saturation limit on the joint to achieve the optimal effect of force control.

[0247] For example, θ sim and θ are the simulated angle and the actual angle of the joint respectively, the computer device calculates the difference between the simulated angle and the actual angle of the joint, obtains a second difference, and determines the proportional calculation result based on the product of the second difference and the position controller stiffness corresponding to the proportional calculation. At this time, the computer device obtains the maximum torque of the position loop corresponding to the proportional calculation.

[0248] Based on this, the second sub-torque τ is determined by the following formula: p :

[0249] τ p =limit(-τ pmax ,Kp*(θ sim -θ),τ pmax )

[0250] Among them, θ sim represents the simulation angle, θ represents the actual angle, Kp is the position controller stiffness corresponding to the proportional calculation, τ pmax Calculates the maximum torque of the position loop corresponding to the ratio. limit(.) is a limiting function.

[0251] Step S1508, performing differential calculation according to the simulated angular velocity and the actual angular velocity of the joint to obtain the third sub-torque.

[0252] Optionally, for each joint, the computer device performs differential calculation according to the simulated angular velocity and the actual angular velocity of the joint to obtain a differential calculation result, and performs restriction processing according to the differential calculation result to obtain the third sub-torque.

[0253] Exemplarily, for each joint, the computer device calculates the difference between the simulated angular velocity and the actual angular velocity of the joint to obtain a third difference, and performs a differential calculation on the third difference to obtain a differential calculation result. The computer device inputs the differential calculation result into the limit function to obtain a third sub-torque. The limit function is used to perform saturation limit on the joint to achieve the optimal effect of force control.

[0254] For example, and The computer device calculates the difference between the simulated angular velocity and the actual angular velocity of the joint to obtain a third difference, and determines the differential calculation result based on the product of the third difference and the position controller stiffness corresponding to the differential calculation. At this time, the computer device obtains the maximum torque of the position loop corresponding to the differential calculation.

[0255] Based on this, the third sub-torque τ is determined by the following formula: d :

[0256]

[0257] in, and are the simulated angular velocity and actual angular velocity of the joint, Kd is the position controller stiffness corresponding to the differential calculation, τ dmax The maximum torque of the position loop corresponding to the differential calculation, limit(.) is a limiting function.

[0258] Step S1510, determining the sub-command torques corresponding to the joints according to the first sub-torque, the second sub-torque and the third sub-torque of each joint.

[0259] Optionally, the computer device fuses the first sub-torque, the second sub-torque and the third sub-torque of each joint to determine the sub-command torque corresponding to each joint.

[0260] In the process of steps S1502 to S1510, if Fig.16As shown, it is a flow chart of determining a new command torque step in an embodiment. Specifically, the computer device determines the actual angle and actual angular velocity corresponding to each joint. For each joint, the computer device calculates the difference between the simulated angle and the actual angle of the joint, obtains a first difference, and performs integral calculation on the first difference to obtain an integral calculation result. The computer device inputs the integral calculation result into the restriction function to obtain a first sub-torque. For each joint, the computer device calculates the difference between the simulated angle and the actual angle of the joint, obtains a second difference, and performs proportional calculation on the second difference to obtain a proportional calculation result. The computer device inputs the proportional calculation result into the restriction function to obtain a second sub-torque. For each joint, the computer device calculates the difference between the simulated angular velocity and the actual angular velocity of the joint, obtains a third difference, and performs differential calculation on the third difference to obtain a differential calculation result. The computer device inputs the differential calculation result into the restriction function to obtain a third sub-torque. The computer device superimposes the first sub-torque, the second sub-torque and the third sub-torque of each joint to determine the sub-command torque corresponding to each joint. The computer device determines a new command torque according to the sub-command torques of each joint, and controls the target device to move at the abnormal position according to the new command torque.

[0261] In this embodiment, according to the simulation angle and simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint can be determined timely and accurately through integral calculation, proportional calculation and differential calculation, so that an effective and accurate new command torque can be obtained.

[0262] In some embodiments, Fig.17 As shown, it is a flow chart of the step of determining the command torque in one embodiment. The implementation subject of this embodiment is a dynamic control unit of a computer device. Specifically, the dynamic control unit in the computer device determines the expected angle corresponding to each joint according to the expected deviation angle and the initial angle corresponding to each joint. Based on the expected angle and the actual angle corresponding to each joint, the simulation angle of each joint is determined through simulation processing. According to the simulation angle of each joint, the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory is determined. According to the simulation angle and the simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint is determined through dynamic control. The dynamic control unit in the computer device determines a new command torque according to the sub-command torque of each joint.

[0263] Wherein, according to the simulation angle and simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint is determined by dynamic control, including: determining the actual angle and actual angular velocity corresponding to each joint. For each joint, an integral calculation is performed according to the simulation angle and the actual angle of the joint to obtain a first sub-torque. A proportional calculation is performed according to the simulation angle and the actual angle of the joint to obtain a second sub-torque. A differential calculation is performed according to the simulation angular velocity and the actual angular velocity of the joint to obtain a third sub-torque. According to the first sub-torque, the second sub-torque and the third sub-torque of each joint, the sub-command torque corresponding to each joint is determined.

[0264] In this embodiment, the expected angle corresponding to each joint can be determined in real time through the expected deviation angle and initial angle corresponding to each joint. The computer device can accurately determine the simulation angle of each joint based on the expected angle and actual angle corresponding to each joint. Based on this, the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory is determined according to the simulation angle of each joint. In this way, according to the simulation angle and simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint can be determined in a timely and accurate manner through integral calculation, proportional calculation and differential calculation, thereby obtaining an effective and accurate new command torque.

[0265] In one embodiment, in order to facilitate a clearer understanding of the technical solution of the present application, a more detailed embodiment is provided for description. The process of this embodiment is as follows Fig.18 The figure is a schematic diagram of a force control process in an embodiment. The computer device includes a core control component, which includes a life judgment unit, a constraint control unit and a dynamic control unit.

[0266] Specifically, the computer device obtains a command signal sent by the operating workbench, and the command signal carries the initial angles of each joint of the target device. The computer device determines the command torque at that time according to the initial angles of each joint of the target device, and controls the movement of the target device according to the command torque. The computer device determines the target detection method. The joint parameters corresponding to each joint of the target device are obtained; the joint parameters include the actual angle and actual angular velocity of the joint. According to the joint parameters corresponding to each joint, the theoretical contact force at the end of the target device is calculated using the target detection method. The computer device actually detects the contact force generated after the target device moves to obtain the actual contact force.

[0267] If the difference between the theoretical contact force and the actual contact force is not within the threshold range, the life judgment unit in the computer device obtains the gain matrix, and performs gain processing on the difference through the gain matrix to obtain the gain deviation external force corresponding to each joint. Among them, the gain matrix includes the gain parameters corresponding to each joint. For each joint, the life judgment unit in the computer device obtains the life curve corresponding to the service life of the joint, and the life curve characterizes the relationship between the deviation external force threshold and the actual angle. For each joint, the life judgment unit in the computer device determines the deviation external force threshold of the joint according to the actual angle and life curve of the joint. The life curve also includes the direction of the force corresponding to the deviation external force threshold. The life judgment unit in the computer device compares the deviation external force threshold of the joint and the gain deviation external force value, and determines the smaller value. If the smaller value is the value of the gain deviation external force of the joint, the life judgment unit in the computer device uses the gain deviation external force of the joint as the setting deviation external force of the joint. If the smaller value is the deviation external force threshold for the joint, the life judgment unit in the computer device determines the direction of the force corresponding to the deviation external force threshold for the joint, and determines the set deviation external force for the joint based on the direction of the force corresponding to the deviation external force threshold for the joint and the deviation external force threshold.

[0268] The constraint control unit in the computer device calculates the sum of the squares of the set deviation external forces of each joint to obtain the sum value, and performs square root calculation on the sum value to obtain the square root value, and uses the inverse of the square root value as the weight of each joint. For each joint, the constraint control unit in the computer device uses the product of the square value of the set deviation external force of the joint and the weight as the uniform deviation external force of the joint. The constraint control unit in the computer device performs Jacobian matrix transposition processing on the uniform deviation external force of the joint to obtain the limit deviation torque of the joint. At least one admittance control processing is performed based on the limit deviation torque of the joint to obtain the initial expected deviation angle of the joint. The constraint control unit in the computer device determines whether the initial expected deviation angle of the joint is within a preset threshold range, and determines the expected deviation angle of the joint based on the judgment result.

[0269] The dynamic control unit in the computer device determines the expected angle corresponding to each joint according to the expected deviation angle and initial angle corresponding to each joint. Based on the expected angle and actual angle corresponding to each joint, the simulation angle of each joint is determined through simulation processing. According to the simulation angle of each joint, the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory is determined. The actual angle and actual angular velocity corresponding to each joint are determined. For each joint, an integral calculation is performed according to the simulation angle and actual angle of the joint to obtain the first sub-torque. A proportional calculation is performed according to the simulation angle and actual angle of the joint to obtain the second sub-torque. A differential calculation is performed according to the simulation angular velocity and actual angular velocity of the joint to obtain the third sub-torque. According to the first sub-torque, the second sub-torque and the third sub-torque of each joint, the sub-command torque corresponding to each joint is determined. The dynamic control unit in the computer device determines the command torque according to the sub-command torque of each joint.

[0270] The computer device uses the new command torque as the next command torque, returns to control the movement of the target device according to the command torque and continues to execute until the difference is within the threshold range, so as to achieve dynamic balance control of the target device.

[0271] In this embodiment, by acquiring the command signal sent by the operating workbench, the command signal carries the initial angle of each joint of the target instrument; the command torque of the current time is determined according to the initial angle of each joint of the target instrument, and the target instrument movement is controlled according to the command torque. In this way, by theoretically calculating and actually detecting the contact force generated after the target instrument moves, the theoretical contact force and the actual contact force can be obtained in real time and accurately. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined according to the initial angle and the difference. That is, the difference is used to evaluate whether the control of the target instrument at the current time reaches dynamic balance. If the difference is not within the threshold range, it means that dynamic balance has not been reached. At this time, the new command torque is used as the next command torque, and the target instrument movement is controlled according to the command torque and continues to be executed until the difference is within the threshold range to stop, so as to realize the dynamic balance control of the target instrument, so that the instrument reaches the most uniform control state, ensuring that the instrument can be stably controlled. In addition, when the target instrument reaches dynamic balance, the service life of the instrument can be optimized.

[0272] In one embodiment, a teleoperation system is provided, the teleoperation system comprising:

[0273] An operating table, used to generate a command signal, wherein the command signal carries the initial angle of each joint of the target device;

[0274] A controller is used to obtain a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device; determine the command torque at that time according to the initial angles of each joint of the target device, and control the movement of the target device according to the command torque; perform theoretical calculation and actual detection on the contact force generated after the movement of the target device, respectively, to obtain a theoretical contact force and an actual contact force; if the difference between the theoretical contact force and the actual contact force is not within a threshold range, determine a new command torque according to the initial angle and the difference; use the new command torque as the next command torque, return to the control of the movement of the target device according to the command torque and continue to execute until the difference is within the threshold range and stop, so as to achieve dynamic balance control of the target device.

[0275] Regarding the specific implementation of the teleoperation system, please refer to the embodiment described in the control method of the instrument. In the teleoperation system, the controller (i.e., the computer device mentioned above) obtains the command signal sent by the operation workbench, and the command signal carries the initial angle of each joint of the target instrument; the command torque of the current time is determined according to the initial angle of each joint of the target instrument, and the target instrument movement is controlled according to the command torque. In this way, by theoretically calculating and actually detecting the contact force generated after the movement of the target instrument, the theoretical contact force and the actual contact force can be obtained in real time and accurately. If the difference between the theoretical contact force and the actual contact force is not within the threshold range, a new command torque is determined according to the initial angle and the difference. That is, the difference is used to evaluate whether the control of the target instrument at the time reaches dynamic balance. If the difference is not within the threshold range, it means that dynamic balance has not been achieved. At this time, the new command torque is used as the next command torque, and the target instrument movement is controlled according to the command torque to continue to execute until the difference is within the threshold range to stop, so as to achieve dynamic balance control of the target instrument, so that the instrument reaches the most uniform control state, ensuring that the instrument can be stably controlled.

[0276] It should be understood that, although the various steps in the flowcharts involved in the above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0277] Based on the same inventive concept, the embodiment of the present application also provides a device control apparatus for implementing the device control method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in the embodiments of the device control apparatus for one or more devices provided below can refer to the limitations of the device control method above, and will not be repeated here.

[0278] In one embodiment, Fig.19 As shown, a control device 1900 of an instrument is provided, comprising: an acquisition module 1902, a control module 1904, an acquisition module 1906, a determination module 1908 and a return module 1910, wherein:

[0279] The acquisition module 1902 is used to acquire the command signal sent by the operating workbench, and the command signal carries the initial angle of each joint of the target device.

[0280] The control module 1904 is used to determine the current command torque according to the initial angles of each joint of the target device, and control the movement of the target device according to the command torque.

[0281] The acquisition module 1906 is used to perform theoretical calculation and actual detection on the contact force generated after the target device moves, so as to obtain the theoretical contact force and the actual contact force.

[0282] The determination module 1908 is configured to determine a new command torque according to the initial angle and the difference if the difference between the theoretical contact force and the actual contact force is not within a threshold range.

[0283] The return module 1910 is used to use the new command torque as the next command torque, and return to control the movement of the target device according to the command torque until the difference is within a threshold range, so as to achieve dynamic balance control of the target device.

[0284] In some embodiments, the device further includes a calculation module, which is used to determine the target detection method. Obtain joint parameters corresponding to each joint of the target device; the joint parameters include the actual angle and actual angular velocity of the joint. According to the joint parameters corresponding to each joint, the target detection method is used to calculate the theoretical contact force at the end of the target device.

[0285] In some embodiments, the determination module 1908 is used to perform gain processing on the difference between the theoretical contact force and the actual contact force to obtain the gain deviation external force corresponding to each joint if the difference between the theoretical contact force and the actual contact force is not within the threshold range. Detect the actual angle of each joint. For each joint, determine the corresponding deviation external force threshold based on the actual angle of the joint, and determine the set deviation external force for the joint according to the deviation external force threshold and the gain deviation external force value of the joint. Perform at least one admittance control process according to the set deviation external force of the joint to obtain the expected deviation angle of the joint. Determine a new command torque based on the expected deviation angle, initial angle and actual angle of each joint.

[0286] In some embodiments, the determination module 1908 is used to obtain, for each joint, a life curve corresponding to the service life of the joint, wherein the life curve represents the relationship between the deviation external force threshold and the actual angle. For each joint, the deviation external force threshold of the joint is determined according to the actual angle of the joint and the life curve.

[0287] In some embodiments, the determination module 1908 is used to compare the deviation external force threshold value and the gain deviation external force value of the joint, and determine the set deviation external force for the joint based on the smaller value.

[0288] In some embodiments, the determination module 1908 is used to determine the uniform deviation external force of the joint according to the weight of the joint and the set deviation external force. The uniform deviation external force of the joint is subjected to transposition processing of the Jacobian matrix to obtain the limiting deviation torque of the joint. At least one admittance control processing is performed based on the limiting deviation torque of the joint to obtain the initial expected deviation angle of the joint. According to the initial expected deviation angle of the joint, the expected deviation angle of the joint is determined; the expected deviation angle is within a preset threshold range.

[0289] In some embodiments, the determination module 1908 is used to perform first-order admittance control processing on the limit deviation torque of the targeted joint through a first-order admittance model to obtain a first deviation angle. Perform second-order admittance control processing on the limit deviation torque of the targeted joint through a second-order admittance model to obtain a second deviation angle. The first deviation angle and the second deviation angle of the targeted joint are fused to obtain an initial desired deviation angle of the targeted joint.

[0290] In some embodiments, the determination module 1908 is used to determine the expected angle corresponding to each joint according to the expected deviation angle and the initial angle corresponding to each joint. Based on the expected angle and the actual angle corresponding to each joint, the simulation angle of each joint is determined. According to the simulation angle of each joint, the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory is determined. According to the simulation angle and the simulation angular velocity corresponding to each joint, the sub-command torque corresponding to each joint is determined. According to the sub-command torque corresponding to each joint, a new command torque is determined.

[0291] In some embodiments, the determination module 1908 is used to determine the actual angle and actual angular velocity corresponding to each joint. For each joint, an integral calculation is performed based on the simulated angle and the actual angle of the joint to obtain a first sub-torque. A proportional calculation is performed based on the simulated angle and the actual angle of the joint to obtain a second sub-torque. A differential calculation is performed based on the simulated angular velocity and the actual angular velocity of the joint to obtain a third sub-torque. Based on the first sub-torque, the second sub-torque and the third sub-torque of each joint, the sub-command torque corresponding to each joint is determined.

[0292] Each module in the control device of the above-mentioned apparatus can be implemented in whole or in part by software, hardware or a combination thereof. Each of the above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory in the computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0293] In one embodiment, a computer device is provided, which may be a server or a terminal. The computer device may be a controller deployed in a teleoperation system, and its internal structure diagram may be as shown in FIG. Fig. 20 As shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, referred to as I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The input / output interface of the computer device is used to exchange information between the processor and an external device. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a control method of an instrument is implemented.

[0294] Those skilled in the art will understand that Fig. 20The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0295] In one embodiment, a computer device is further provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above method embodiments when executing the computer program.

[0296] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments are implemented.

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

[0298] 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, stored data, displayed data, 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 relevant data must comply with relevant laws, regulations and standards of relevant countries and regions.

[0299] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present 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), magnetoresistive 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. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0300] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, 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.

[0301] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A method for controlling an apparatus, It is characterized in that The method comprises: Acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device; Determining the current command torque according to the initial angles of each joint of the target device, and controlling the movement of the target device according to the command torque; The contact force generated by the movement of the target device is theoretically calculated and actually tested to obtain a theoretical contact force and an actual contact force; If the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference; The new command torque is used as the next command torque, and the control of the target device movement according to the command torque is returned to continue to be executed until the difference is within the threshold range, so as to achieve dynamic balance control of the target device; The difference between the theoretical contact force and the actual contact force is a deviation external force; if the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference, including: If the difference between the theoretical contact force and the actual contact force is not within the threshold range, then based on the difference, the set deviation external force corresponding to each joint is determined; according to the set deviation external force of each joint, the expected deviation angle of each joint is determined; according to the expected deviation angle and the initial angle, a new command torque is determined, wherein the set deviation external force is a deviation external force within the deviation external force threshold.

2. The method according to claim 1, It is characterized in that The calculation steps of the theoretical contact force include: Determine the target detection method; Acquire joint parameters corresponding to each joint of the target device; the joint parameters include actual angle and actual angular velocity of the joint; According to the joint parameters corresponding to each joint, the theoretical contact force of the end of the target instrument is calculated using the target detection method.

3. The method according to claim 1, It is characterized in that If the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference, comprising: If the difference between the theoretical contact force and the actual contact force is not within the threshold range, performing gain processing on the difference to obtain the gain deviation external force corresponding to each joint; Detect the actual angle of each joint; For each joint, a corresponding deviation external force threshold is determined based on the actual angle of the joint, and a set deviation external force of the joint is determined according to the deviation external force threshold of the joint and the value of the gain deviation external force; Perform at least one admittance control process according to the set deviation external force of the joint to obtain a desired deviation angle of the joint; The new command torque is determined based on the expected deviation angle, initial angle and actual angle of each joint.

4. The method according to claim 3, It is characterized in that The step of determining the corresponding deviation external force threshold for each joint based on the actual angle of the joint comprises: For each joint, a life curve corresponding to the service life of the joint is obtained, wherein the life curve represents the relationship between the deviation external force threshold and the actual angle; For each joint, the deviation external force threshold of the joint is determined according to the actual angle and life curve of the joint.

5. The method according to claim 3, It is characterized in that Determining the set deviation external force for the joint according to the deviation external force threshold value and the gain deviation external force value of the joint includes: The deviation external force threshold value and the gain deviation external force value of the joint are compared, and the set deviation external force of the joint is determined based on the smaller value.

6. The method according to claim 3, It is characterized in that The step of performing at least one admittance control process according to the set deviation external force of the joint to obtain the desired deviation angle of the joint includes: Determine the uniform deviation external force of the targeted joint according to the weight of the targeted joint and the set deviation external force; The Jacobian matrix is ​​transposed on the uniform deviation external force of the targeted joint to obtain the limiting deviation torque of the targeted joint; Perform at least one admittance control process based on the limit deviation torque of the targeted joint to obtain an initial desired deviation angle of the targeted joint; According to the initial expected deviation angle of the targeted joint, an expected deviation angle of the targeted joint is determined; the expected deviation angle is within a preset threshold range.

7. The method according to claim 6, It is characterized in that The step of performing at least one admittance control process based on the limit deviation torque of the joint to obtain an initial expected deviation angle of the joint includes: Performing first-order admittance control processing on the limiting deviation torque of the targeted joint through a first-order admittance model to obtain a first deviation angle; Performing second-order admittance control processing on the limiting deviation torque of the targeted joint through a second-order admittance model to obtain a second deviation angle; The first deviation angle and the second deviation angle of the targeted joint are fused to obtain an initial expected deviation angle of the targeted joint.

8. The method according to claim 3, It is characterized in that Determining a new command torque according to the expected deviation angle, initial angle and actual angle of each joint includes: Determine the desired angle corresponding to each joint according to the desired deviation angle and the initial angle corresponding to each joint; Determine the simulation angle of each joint based on the expected angle and the actual angle corresponding to each joint; According to the simulation angle of each joint, determine the simulation angular velocity of each joint at the current moment when it moves according to the preset motion trajectory; Determine the sub-command torque corresponding to each joint according to the simulation angle and simulation angular velocity corresponding to each joint; Determine the new command torque according to the sub-command torque corresponding to each joint.

9. The method according to claim 8, It is characterized in that Determining the sub-command torques corresponding to the joints according to the simulation angles and simulation angular velocities corresponding to the joints includes: Determine the actual angle and actual angular velocity corresponding to each joint; For each joint, an integral calculation is performed according to the simulation angle and the actual angle of the joint to obtain a first sub-torque; A second sub-torque is obtained by performing proportional calculation based on the simulated angle and the actual angle of the targeted joint; The third sub-torque is obtained by performing differential calculation according to the simulated angular velocity and the actual angular velocity of the joint; According to the first sub-torque, the second sub-torque and the third sub-torque of each joint, the sub-command torques corresponding to the joints are determined.

10. A control device for an apparatus, It is characterized in that The device comprises: An acquisition module, used to acquire a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device; A control module, used to determine the current command torque according to the initial angles of each joint of the target device, and control the movement of the target device according to the command torque; An acquisition module, used for theoretically calculating and actually detecting the contact force generated by the target device after the movement, to obtain a theoretical contact force and an actual contact force; a determination module, configured to determine a new command torque according to the initial angle and the difference if the difference between the theoretical contact force and the actual contact force is not within a threshold range; A return module, used for taking the new command torque as the next command torque, returning to the control of the target device movement according to the command torque and continuing to execute until the difference is within a threshold range, so as to achieve dynamic balance control of the target device; The difference between the theoretical contact force and the actual contact force is the deviation external force; the determination module is also used to determine the set deviation external force corresponding to each joint based on the difference if the difference between the theoretical contact force and the actual contact force is not within a threshold range; determine the expected deviation angle of each joint according to the set deviation external force of each joint; determine a new command torque according to the expected deviation angle and the initial angle, wherein the set deviation external force is a deviation external force within the deviation external force threshold.

11. A teleoperation system, It is characterized in that The system comprises: An operating table, used to generate a command signal, wherein the command signal carries the initial angle of each joint of the target device; A controller, used for acquiring a command signal sent by the operating workbench, wherein the command signal carries the initial angles of each joint of the target device; determining a command torque at that time according to the initial angles of each joint of the target device, and controlling the movement of the target device according to the command torque; performing theoretical calculation and actual detection on the contact force generated after the movement of the target device, respectively, to obtain a theoretical contact force and an actual contact force; if the difference between the theoretical contact force and the actual contact force is not within a threshold range, determining a new command torque according to the initial angle and the difference; using the new command torque as the next command torque, returning to the control of the movement of the target device according to the command torque and continuing to execute until the difference is within the threshold range and stops, so as to achieve dynamic balance control of the target device; The difference between the theoretical contact force and the actual contact force is the deviation external force; the controller is also used to determine the set deviation external force corresponding to each joint based on the difference if the difference between the theoretical contact force and the actual contact force is not within a threshold range; determine the expected deviation angle of each joint according to the set deviation external force of each joint; determine a new command torque according to the expected deviation angle and the initial angle, wherein the set deviation external force is a deviation external force within the deviation external force threshold.

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

Images