A needle-punching robot control method and related device

Abstract

The application provides a needle acupuncture robot control method and related device, including: obtaining a twist angle speed difference value and an insertion speed difference value, calculating a disturbance force difference value and a disturbance torque difference value according to the twist angle speed difference value and a disturbance formula, and compensating for the disturbance force and the disturbance torque generated by static and dynamic imbalance. The target model is used for disturbance compensation. Specifically, the twist angle speed difference value, the insertion speed difference value, the disturbance force difference value, the disturbance torque difference value and the target model are used to calculate the applied force difference value and the applied torque difference value of the needle acupuncture robot. The actual applied force and the actual applied torque of the needle acupuncture robot are adjusted by using the calculated applied force difference value and the applied torque difference value. The disturbance generated by the static and dynamic imbalance is compensated for. The actual twist angle speed during traditional Chinese medicine acupuncture is the expected twist angle speed, and the actual insertion speed is the expected insertion speed. Finally, high-precision control of the insertion speed and the twist angle speed during the operation of the needle acupuncture robot is realized.

Description

Technical Field

[0001] This invention relates to the field of computers, and in particular to a method and apparatus for controlling an acupuncture robot. Background Technology

[0002] With the development of computer and automation technologies, acupuncture robots can now be used to perform traditional Chinese medicine acupuncture on patients. When performing acupuncture, the acupuncture robot can perform lifting and thrusting operations or twisting operations. When performing these operations, there are standardized requirements for insertion speed, twisting angular velocity, and needle insertion depth.

[0003] The needle-piercing robot includes an end-effector platform that holds the needle to be pierced. The robot rotates the end-effector platform to achieve needle twisting. However, because the geometric center of the needle, the gripping platform, and the robot's arm are not aligned with its center of mass during installation, static and dynamic imbalances occur. These disturbances, along with their torques, generate micro-vibrations. This micro-vibration dynamic environment affects the attitude accuracy and stability of the robot's high-precision components, leading to a decrease in the robot's control precision. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a control method and related device for acupuncture robots, which can compensate for the disturbance force and disturbance torque caused by static-dynamic imbalance, thereby improving the control accuracy of acupuncture robots.

[0005] To achieve the above objectives, this application provides the following technical solution:

[0006] This application provides a method for controlling an acupuncture robot, including:

[0007] The difference between the expected and actual angular velocity of the acupuncture robot during traditional Chinese medicine acupuncture, as well as the difference between the expected and actual insertion speed, are obtained.

[0008] The difference in perturbation force and the difference in perturbation torque are calculated based on the difference in twist angular velocity and the perturbation formula. The perturbation formula includes static unbalance and inertia matrix.

[0009] The applied force difference and applied torque difference of the needle-piercing robot are calculated based on the twisting angular velocity difference, the insertion velocity difference, the disturbance force difference, the disturbance torque difference, and the target model. The target model is the kinematic and dynamic model of the end-effector platform of the needle-piercing robot.

[0010] The actual applied force and actual applied torque are adjusted according to the difference in applied force and the difference in applied torque, so as to control the actual twisting angular velocity of the acupuncture robot to be the desired twisting angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0011] Optionally, the method further includes:

[0012] The target model is constructed, which includes multiple physical quantities to be calculated and multiple known physical quantities. The multiple physical quantities to be calculated include at least historical perturbation forces and historical perturbation moments.

[0013] The historical disturbance force and the historical disturbance torque are calculated based on the known physical quantities and the target model.

[0014] The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, and the disturbance formula.

[0015] Optionally, the known physical quantities include historical applied force, historical applied torque, and historical twist angular velocity;

[0016] The method further includes:

[0017] The historical applied force and the historical applied torque are obtained from the force / torque sensor;

[0018] The historical twist angular velocity is obtained from the gyroscope;

[0019] The step of calculating the static unbalance and the inertia matrix based on the historical disturbance force, the historical disturbance moment, and the disturbance formula includes:

[0020] The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, the historical twist angular velocity, and the disturbance formula.

[0021] Optionally, the known physical quantities include historical insertion velocity, historical needle tip force, and historical axial friction force;

[0022] The method further includes:

[0023] The historical insertion velocity is obtained from the accelerometer;

[0024] The historical needle tip force was calculated based on the historical insertion velocity and the needle tip force formula.

[0025] The historical axial friction force was calculated based on the historical needle tip force and axial friction force formulas.

[0026] The step of calculating the historical disturbance force and the historical disturbance moment based on the known physical quantities and the target model includes:

[0027] The historical disturbance force and the historical disturbance torque are calculated based on the historical needle tip force, the historical axial friction force, the historical twisting angular velocity, the historical applied force, the historical applied torque, and the target model.

[0028] Optionally, the needle tip force formula includes multiple coefficients and multiple physical quantities to be detected. The multiple coefficients include the elastic coefficient of the skin, the viscosity coefficient of the skin, the elastic coefficient of the muscle, and the viscosity coefficient of the muscle. The multiple physical quantities to be detected include the insertion depth of the skin layer, the basic deformation distance of the skin, the insertion depth of the muscle layer, and the tissue deformation.

[0029] Optionally, the step of calculating the disturbance force difference and disturbance torque difference based on the twist angular velocity difference and the disturbance formula includes:

[0030] The desired disturbance force and desired disturbance torque are calculated based on the desired twist angular velocity and the disturbance formula, and the actual disturbance force and actual disturbance torque are calculated based on the actual twist angular velocity and the actual insertion velocity.

[0031] The disturbance force difference is obtained based on the expected disturbance force and the actual disturbance force, and the disturbance torque difference is obtained based on the expected disturbance torque and the actual disturbance torque.

[0032] The calculation of the applied force difference and applied torque difference of the needle-piercing robot based on the twisting angular velocity difference, the insertion velocity difference, the disturbance force difference, the disturbance torque difference, and the target model includes:

[0033] The desired applied force and desired applied torque of the needle-piercing robot are calculated based on the desired twisting angular velocity, the desired insertion velocity, the desired disturbance force and the desired disturbance torque, and the target model.

[0034] The actual applied force and actual applied torque are obtained from the force / torque sensor;

[0035] The difference between the applied force and the actual applied force is obtained based on the expected applied force and the actual applied force; the difference between the applied torque and the expected applied torque is obtained based on the actual applied torque.

[0036] The step of adjusting the actual applied force and actual applied torque based on the applied force difference and the applied torque difference to control the actual rotational angular velocity of the acupuncture robot during traditional Chinese medicine acupuncture to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed includes:

[0037] The actual applied force is adjusted to the desired applied force based on the applied force difference, and the actual applied torque is adjusted to the desired applied force based on the applied torque difference, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0038] This application provides a control device for an acupuncture robot, comprising:

[0039] The first acquisition unit is used to acquire the difference between the expected and actual twisting angular velocities of the acupuncture robot when performing traditional Chinese medicine acupuncture, as well as the difference between the expected and actual insertion speeds.

[0040] The first calculation unit is used to calculate the difference in disturbance force and the difference in disturbance torque based on the difference in twist angular velocity and the disturbance formula. The disturbance formula includes static unbalance and inertia matrix.

[0041] The second calculation unit is used to calculate the applied force difference and applied torque difference of the needle-piercing robot based on the twisting angular velocity difference, the insertion velocity difference, the disturbance force difference and the disturbance torque difference, and the target model. The target model is the kinematic and dynamic model of the end-effector platform of the needle-piercing robot.

[0042] The control unit is used to adjust the actual applied force and the actual applied torque according to the difference between the applied force and the difference between the applied torque, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0043] Optionally, the device further includes a third computing unit, the third computing unit being configured to:

[0044] The target model is constructed, which includes multiple physical quantities to be calculated and multiple known physical quantities. The multiple physical quantities to be calculated include at least historical perturbation forces and historical perturbation moments.

[0045] The historical disturbance force and the historical disturbance torque are calculated based on the known physical quantities and the target model.

[0046] The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, and the disturbance formula.

[0047] This application provides a needle-punching robot control device, the device comprising: a processor and a memory;

[0048] The memory is used to store instructions;

[0049] The processor is configured to execute the instructions in the memory and perform the method as described above.

[0050] This application provides a computer-readable storage medium including instructions that, when executed on a computer, cause the computer to perform the methods described above.

[0051] This application provides a control method for an acupuncture robot, comprising: obtaining the difference between the desired and actual rotational angular velocities of the acupuncture robot during traditional Chinese medicine acupuncture, and the difference between the desired and actual insertion speeds; calculating the difference in disturbance force and disturbance torque based on the difference in rotational angular velocities and a disturbance formula, wherein the disturbance formula includes static imbalance and inertia matrix; and compensating for the disturbance force and disturbance torque generated by static-dynamic imbalance after calculating the difference in disturbance force and disturbance torque. Disturbance compensation can be achieved using the kinematic and dynamic model of the end-effector platform of the acupuncture robot, i.e., the target model. Specifically, the applied force difference and applied torque difference of the acupuncture robot can be calculated based on the difference in twisting angular velocity, insertion velocity, disturbance force, and disturbance torque, as well as the target model. The actual applied force and torque of the acupuncture robot can be adjusted to the desired applied force and torque using the calculated applied torque difference, thereby compensating for the disturbance caused by static-dynamic imbalance. This achieves the goal of matching the actual twisting angular velocity with the desired twisting angular velocity and the actual insertion velocity with the desired insertion velocity during acupuncture, ultimately enabling high-precision control of the insertion velocity and twisting angular velocity during the operation of the acupuncture robot. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0053] Figure 1 A flowchart illustrating a needle-punching robot control method provided in an embodiment of this application is shown;

[0054] Figure 2 A schematic diagram of a flowchart for calculating the parameters of a target model according to an embodiment of this application is shown;

[0055] Figure 3 This paper shows a schematic diagram of the structure of a needle-punching robot control device provided in an embodiment of this application;

[0056] Figure 4A schematic diagram of another needle-punching robot control device provided in an embodiment of this application is shown. Detailed Implementation

[0057] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the specific embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0058] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0059] The needle-piercing robot includes an end-effector platform that holds the needle to be pierced. The robot rotates the end-effector platform to achieve needle twisting. However, because the geometric center of the needle, the gripping platform, and the robot's arm are not aligned with its center of mass during installation, static and dynamic imbalances occur. These disturbances, along with their torques, generate micro-vibrations. This micro-vibration dynamic environment affects the attitude accuracy and stability of the robot's high-precision components, leading to a decrease in the robot's control precision.

[0060] Therefore, in order to achieve high-precision control of the needle-piercing robot, it is necessary to compensate for the disturbance force and disturbance torque generated by static-dynamic imbalance.

[0061] Based on this, this application provides a control method for an acupuncture robot, comprising: obtaining the difference between the desired and actual rotational angular velocities of the acupuncture robot during traditional Chinese medicine acupuncture, and the difference between the desired and actual insertion speeds; calculating the difference in disturbance force and the difference in disturbance torque based on the difference in rotational angular velocities and a disturbance formula, wherein the disturbance formula includes static imbalance and inertia matrix; and after calculating the difference in disturbance force and the difference in disturbance torque, compensation can be made for the disturbance force and disturbance torque generated by static and dynamic imbalance. Disturbance compensation can be achieved using the kinematic and dynamic model of the end-effector platform of the acupuncture robot, i.e., the target model. Specifically, the applied force difference and applied torque difference of the acupuncture robot can be calculated based on the difference in twisting angular velocity, insertion velocity, disturbance force, and disturbance torque, as well as the target model. The actual applied force and torque of the acupuncture robot can be adjusted to the desired applied force and torque using the calculated applied torque difference, thereby compensating for the disturbance caused by static-dynamic imbalance. This achieves the goal of matching the actual twisting angular velocity with the desired twisting angular velocity and the actual insertion velocity with the desired insertion velocity during acupuncture, ultimately enabling high-precision control of the insertion velocity and twisting angular velocity during the operation of the acupuncture robot.

[0062] To better understand the technical solution and effects of this application, the specific embodiments will be described in detail below with reference to the accompanying drawings.

[0063] refer to Figure 1 The diagram shown is a flowchart illustrating a needle-punching robot control method provided in an embodiment of this application. The method includes the following steps:

[0064] S101, obtain the difference between the expected and actual twisting angular velocities of the acupuncture robot when performing traditional Chinese medicine acupuncture, as well as the difference between the expected and actual insertion speeds.

[0065] In the embodiments of this application, the acupuncture robot inserts needles into the patient's body during traditional Chinese medicine acupuncture. The desired angular velocity of the needle and the desired insertion velocity at different insertion positions are obtained according to standardized requirements. The actual angular velocity of the needle and the actual insertion velocity at different insertion positions can also be obtained. The desired angular velocity of rotation is the angular velocity at which the acupuncture robot intends to insert the needle, while the actual angular velocity of rotation is the angular velocity measured in real-time during insertion, such as by a gyroscope. The angular velocity of rotation can also be referred to as the rotation vector of the acupuncture robot's end effector platform during the rotation operation. The desired insertion velocity is the insertion velocity at which the acupuncture robot intends to insert the needle, while the actual insertion velocity is the insertion velocity measured in real-time during insertion, such as by an accelerometer. The insertion velocity of insertion can also be referred to as the movement velocity of the acupuncture robot's end effector platform.

[0066] The difference between the desired and actual twisting angular velocities, as well as the difference between the desired and actual insertion speeds, can be calculated. Subsequently, the difference in applied force and torque can be calculated using these differences, so as to control the twisting and insertion speeds of the needle body.

[0067] S102, the difference in disturbance force and the difference in disturbance torque are calculated based on the difference in twist angular velocity and the disturbance formula.

[0068] In the embodiments of this application, in order to compensate for the disturbance force and disturbance torque generated by static-dynamic imbalance, the disturbance force difference and disturbance torque difference corresponding to the twist angular velocity difference can be calculated first. The disturbance force difference refers to the difference between the desired disturbance force and the actual disturbance force, and the disturbance torque difference refers to the difference between the desired disturbance torque and the actual disturbance torque. Once the disturbance force difference and disturbance torque difference are calculated, the applied force difference and applied torque difference can be subsequently calculated. Specifically, the disturbance force difference and disturbance torque difference can be calculated based on the twist angular velocity difference and the disturbance formula.

[0069] Specifically, the disturbance formula can include two parameters: static unbalance and inertia matrix. The static unbalance is the vector from the geometric center of the rotating part in the end-effector to its center of mass, and the inertia matrix is ​​the inertia of the rotating part relative to the geometric center to its center of mass. If these two parameters are known, and combined with the twist angular velocity, the disturbance force and disturbance torque can be easily calculated.

[0070] As an example, the perturbation formula can be as follows:

[0071] F e =-m p (ω × ω × ρ)

[0072] T e =((Iω) × ) T +Iω × )ω

[0073] Among them, F e For the disturbance force generated by static imbalance, t e For the disturbance torque generated by dynamic imbalance, m p Let ρ be the mass of the rotating part in the end-clamping platform, ω be the twist angular velocity, ρ be the static unbalance, and I be the inertia matrix. × Let be the cross product matrix of the twist angular velocities, defined as a 3rd-order column vector of the twist angular velocities, where

[0074]

[0075]

[0076] Twist angular velocity can be obtained using a gyroscope. If the twist angular velocity has three degrees of freedom, it can be obtained using a MEMS three-axis gyroscope.

[0077] Therefore, the relationship between the twist angular velocity, the disturbance force, and the disturbance torque can be established using the disturbance formula. Thus, the difference in disturbance force and the difference in disturbance torque can be calculated from the difference in twist angular velocity, so as to compensate for the disturbance caused by static-dynamic imbalance.

[0078] In some embodiments, the specific process of calculating the difference in disturbance force and the difference in disturbance torque based on the difference in twist angular velocity and the disturbance formula is as follows:

[0079] The desired disturbance force and desired disturbance torque are calculated based on the desired twist angular velocity and disturbance formula. The actual disturbance force and actual disturbance torque are calculated based on the actual twist angular velocity and actual insertion velocity. Then, the disturbance force difference is calculated based on the desired disturbance force and actual disturbance force, and the disturbance torque difference is calculated based on the desired disturbance torque and actual disturbance torque.

[0080] In other embodiments, when the end-clamping platform twists the needle body, the desired disturbance force and desired disturbance torque generated by static-dynamic imbalance can be calculated using the disturbance formula for the desired twisting angular velocity. Correspondingly, the actual disturbance force and actual disturbance torque generated by static-dynamic imbalance can be calculated using the disturbance formula for the actual twisting angular velocity. In this way, by directly inputting the twisting angular velocity difference into the disturbance formula, the disturbance force difference between the desired disturbance force and the actual disturbance force, as well as the disturbance torque difference between the desired disturbance torque and the actual disturbance torque, can be directly obtained.

[0081] S103, the applied force difference and applied torque difference of the needle-piercing robot are calculated based on the difference in twist angular velocity, the difference in insertion velocity, the difference in disturbance force and the difference in disturbance torque, as well as the target model.

[0082] In the embodiments of this application, in order to compensate for the disturbance force and disturbance torque generated by static-dynamic imbalance, the applied force difference and applied torque difference of the needle-piercing robot can be calculated according to the target model. The target model is the kinematic and dynamic model of the end-effector platform of the needle-piercing robot. The target model gives the mechanical relationship between the applied force and applied torque and the needle position trajectory and twisting angular velocity. The needle position trajectory includes the insertion velocity.

[0083] Specifically, the applied force difference and applied torque difference of the needle-piercing robot can be calculated based on the difference in twisting angular velocity, insertion speed, disturbance force difference, and disturbance torque difference, as well as the target model. In other words, the target model links the applied force difference and applied torque difference, as well as the disturbance force difference and disturbance torque difference, together. It can calculate the applied force and applied torque after disturbance compensation through the disturbance force and disturbance torque. That is, the actual applied force and actual applied torque can be adjusted for disturbance compensation. When the needle is inserted using the adjusted applied force and applied torque, the twisting angular velocity and insertion speed can be controlled more accurately.

[0084] In other words, the difference in disturbance force and the difference in disturbance torque can be calculated first according to the disturbance formula, and the disturbance can be compensated using the target model to obtain the adjusted force and torque applied to the needle body.

[0085] In actual calculations, in order to successfully calculate the difference in applied force and the difference in applied torque, the parameters of the target model and the perturbation formula must be known quantities. For example, the static unbalance and inertia matrix in the perturbation formula should be known quantities.

[0086] As an example, the formula for the target model can be as follows:

[0087]

[0088]

[0089] Where m is the total mass of the end effector platform, and v is the velocity of the end effector platform of the needle-piercing robot. F represents the motion acceleration of the end effector platform of the needle-piercing robot. c To apply force, F T For the force exerted by the needle tip, F N For axial friction, F e For the disturbance force generated by static imbalance, I z Let ω be the rotational inertia matrix of the end effector platform, ω be the rotational angular velocity of the end effector platform of the needle-piercing robot, and ω& be the rotational angular acceleration of the end effector platform. × T is the cross product matrix of the rotational angular velocity of the end-effector platform. c To apply torque, T f For frictional torque, T e The disturbance torque generated by dynamic imbalance.

[0090] In other words, by using the target model, after obtaining the disturbance force and disturbance torque, the applied force and applied torque can be calculated.

[0091] In some embodiments, the specific process of calculating the applied force difference and applied torque difference of the needle-piercing robot based on the twist angular velocity difference, insertion velocity difference, disturbance force difference, disturbance torque difference, and target model is as follows:

[0092] The desired applied force and desired applied torque of the needle-piercing robot are calculated based on the desired twisting angular velocity, desired insertion velocity, desired disturbance force and desired disturbance torque, and the target model. The actual applied force and actual applied torque of the needle-piercing robot are calculated based on the actual twisting angular velocity, actual insertion velocity, actual disturbance force and actual disturbance torque, and the target model. The difference between the applied force and the actual applied force is obtained based on the desired applied force and the actual applied torque.

[0093] In other embodiments, the desired applied force and the desired applied torque can be calculated using a target model, and the actual applied force and the actual applied torque can also be measured using a force / torque sensor. In this way, the difference between the desired applied force and the actual applied force, as well as the difference between the desired applied torque and the actual applied torque, can be directly obtained.

[0094] The static unbalance and inertia matrix in the perturbation formula are calculated. When the applied force, applied torque, friction torque, needle tip force, and axial friction force are known physical quantities, the perturbation force and perturbation torque can be calculated according to the target model, and then the static unbalance and inertia matrix can be calculated according to the perturbation formula.

[0095] The applied force and torque can be acquired using a force / torque sensor. If the applied force and torque have six degrees of freedom, they can be acquired using a six-axis force / torque sensor. When the needle rotates at a fixed angular velocity, the frictional torque can be calculated using data measured by a torque sensor and an angle measuring device.

[0096] The force exerted by the needle tip can be represented by a three-degree-of-freedom vector:

[0097]

[0098] Among them, f T This represents the z-axis component of the needle tip force. The needle tip force can be calculated using the needle tip force formula. The needle tip force formula can be derived using a viscoelastic model, which is divided into five stages: the skin penetration and needle insertion stage, the relatively static state with skin stage, the fat layer stage, the muscle penetration stage, and the muscle friction stage. The specific formulas for these five stages are shown below:

[0099]

[0100] Where, k skin ρ is the elastic coefficient of the skin, x is the penetration depth into the skin layer, and ρ is the penetration depth into the skin layer. skin ν is the viscosity coefficient of the skin. z Let denot be the z-axis component of the insertion velocity, Δx be the basic skin deformation distance, η be the tissue deformation, l be the insertion depth into the muscle layer, and k be the insertion depth into the muscle layer. muscle ρ is the elastic coefficient of the muscle. muscle This represents the viscosity coefficient of the muscle.

[0101] As can be seen from the above formula for needle tip force, the formula includes multiple coefficients and multiple physical quantities to be measured. The multiple coefficients include the elastic coefficient of the skin, the viscosity coefficient of the skin, the elastic coefficient of the muscle, and the viscosity coefficient of the muscle. The multiple physical quantities to be measured include the penetration depth of the skin layer, the basic deformation distance of the skin, the penetration depth of the muscle layer, and the tissue deformation.

[0102] refer to Figure 2 As shown, the elastic coefficient, viscosity coefficient, and muscle elastic coefficient of the skin can be determined using biomechanical experiments. The insertion depth in the skin layer, the basic deformation distance of the skin, the insertion depth in the muscle layer, and the tissue deformation can be calculated using a high-precision optical camera, ultrasound combined with depth and deformation recognition algorithms. The insertion velocity can be obtained from an accelerometer; specifically, the insertion velocity can be obtained by integrating the acceleration signal output by the accelerometer. If the insertion velocity is represented using three degrees of freedom, it can be obtained using a MEMS triaxial accelerometer.

[0103] Axial friction force can also be represented by a three-degree-of-freedom vector:

[0104]

[0105] Among them, f N Let z be the z-axis component of the axial friction force. The axial friction force can be calculated using the needle tip force and the axial friction force formula. The axial friction force formula is shown below:

[0106]

[0107] Among them, l t Let t be the acupuncture path at time t. In the actual calculation process, the axial friction force is calculated not only by considering the friction between the needle body and the needle tip and the tissue, but also by accumulating the mutual friction between the tissue and the needle body during the acupuncture process.

[0108] To calculate the static unbalance and inertia matrix, historical data can be used. For example, the static unbalance and inertia matrix can be calculated based on historical disturbance forces, historical disturbance moments, and disturbance formulas. If the historical disturbance forces and historical disturbance moments are also physical quantities to be calculated, they can be calculated using the target model.

[0109] As one possible approach, to calculate the static unbalance and inertia matrix in the perturbation formula, a target model can be constructed first. The target model includes multiple physical quantities to be calculated and multiple known physical quantities. Among them, the multiple physical quantities to be calculated include at least the historical perturbation force and historical perturbation torque, and the multiple known physical quantities include the historical applied force, historical applied torque, historical twisting angular velocity, historical insertion velocity, historical needle tip force, and historical axial friction force. The historical perturbation force and historical perturbation torque can be calculated based on the known physical quantities and the target model.

[0110] refer to Figure 2 As shown, after measuring the elastic coefficient of the skin, the viscosity coefficient of the skin, the elastic coefficient of the muscle, the viscosity coefficient of the muscle, the insertion depth of the skin layer, the basic deformation distance of the skin, the insertion depth of the muscle layer, the tissue deformation, and the historical insertion speed, the historical needle tip force can be calculated according to the needle tip force formula.

[0111] After calculating the historical needle tip force and needle insertion trajectory, the historical axial friction force can be calculated using the axial friction force formula.

[0112] Historical applied force and torque are obtained from force / torque sensors, and historical twist angular velocity is obtained from gyroscopes. After calculating historical needle tip force and historical axial friction force, historical disturbance force and historical disturbance torque can be calculated based on historical needle tip force, historical axial friction force, historical twist angular velocity, historical applied force, historical applied torque, and the target model.

[0113] After calculating the historical disturbance force and historical disturbance torque, the static unbalance and inertia matrix can be calculated based on the historical disturbance force, historical disturbance torque, historical twist angular velocity, and disturbance formula.

[0114] In other words, the parameters in the target model and the perturbation formula can be calculated based on historical data, so that all parameters are known physical quantities. Subsequently, the perturbation force and perturbation torque can be calculated based on the perturbation formula, and the applied force and applied torque after perturbation compensation can be calculated based on the target model.

[0115] S104, adjust the actual applied force and actual applied torque according to the difference between applied force and applied torque, so as to control the actual twisting angular velocity of the acupuncture robot to be the desired twisting angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0116] In the embodiments of this application, after calculating the applied force difference and applied torque difference based on the difference in twisting angular velocity, the difference in insertion speed, and the target model, the actual applied force and actual applied torque can be adjusted according to the difference in applied force and applied torque. The adjusted applied force and applied torque are the applied force and applied torque after disturbance compensation. Therefore, the adjusted applied force and applied torque can be directly used to control the acupuncture robot to perform traditional Chinese medicine acupuncture. At this time, the actual twisting angular velocity is the desired twisting angular velocity, and the actual insertion speed is the desired insertion speed. In this way, controlling the acupuncture robot to output accurate applied force and applied torque can improve the posture control accuracy during the acupuncture process and achieve the desired twisting angular velocity and insertion speed.

[0117] Specifically, the actual applied force can be adjusted to the desired applied force based on the difference in applied force, and the actual applied torque can be adjusted to the desired applied force based on the difference in applied torque, so as to control the actual rotational angular velocity of the acupuncture robot to the desired rotational angular velocity and the actual insertion speed to the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0118] In practical applications, the end-effector platform can be equipped with an attitude controller to control the attitude of the needle insertion process. The attitude controller can acquire real-time position trajectory, insertion speed, and twisting angular velocity based on the target model and given the needle insertion position trajectory and twisting angular velocity, and control the applied force and torque to achieve high-precision control of the insertion speed and twisting angular velocity. The attitude controller can be a robust controller.

[0119] In practical applications, the target model can be a multi-degree-of-freedom mechanical model, such as a six-degree-of-freedom mechanical model, which enables more precise control of the needle-piercing robot.

[0120] Therefore, the acupuncture robot control method provided in this application includes: obtaining the difference between the desired and actual rotational angular velocities of the acupuncture robot during traditional Chinese medicine acupuncture, as well as the difference between the desired and actual insertion speeds. The difference in disturbance force and disturbance torque can be calculated based on the difference in rotational angular velocities and the disturbance formula, wherein the disturbance formula includes static imbalance and inertia matrix. After calculating the difference in disturbance force and disturbance torque, the disturbance force and disturbance torque generated by static and dynamic imbalance can be compensated. Disturbance compensation can be achieved using the kinematic and dynamic model of the end-effector platform of the acupuncture robot, i.e., the target model. Specifically, the applied force difference and applied torque difference of the acupuncture robot can be calculated based on the difference in twisting angular velocity, insertion velocity, disturbance force, and disturbance torque, as well as the target model. The actual applied force and torque of the acupuncture robot can be adjusted to the desired applied force and torque using the calculated applied torque difference, thereby compensating for the disturbance caused by static-dynamic imbalance. This achieves the goal of matching the actual twisting angular velocity with the desired twisting angular velocity and the actual insertion velocity with the desired insertion velocity during acupuncture, ultimately enabling high-precision control of the insertion velocity and twisting angular velocity during the operation of the acupuncture robot.

[0121] Based on the acupuncture robot control method provided in the above embodiments, this application also provides an acupuncture robot control device, see reference. Figure 3 and Figure 4 The diagram shown is a structural schematic of various acupuncture robot control devices provided in embodiments of this application. The acupuncture robot control device 200 provided in embodiments of this application includes:

[0122] The first acquisition unit 210 is used to acquire the difference between the expected and actual twisting angular velocities of the acupuncture robot when performing traditional Chinese medicine acupuncture, as well as the difference between the expected and actual insertion speeds.

[0123] The first calculation unit 220 is used to calculate the difference in disturbance force and the difference in disturbance torque based on the difference in twist angular velocity and the disturbance formula. The disturbance formula includes static unbalance and inertia matrix.

[0124] The second calculation unit 230 is used to calculate the applied force difference and applied torque difference of the needle robot based on the twisting angular velocity difference, the insertion velocity difference, the disturbance force difference and the disturbance torque difference, and the target model. The target model is the kinematic and dynamic model of the end gripping platform of the needle robot.

[0125] The control unit 240 is used to adjust the actual applied force and the actual applied torque according to the difference between the applied force and the difference between the applied torque, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0126] Optionally, the device further includes a third computing unit 250, the third computing unit 250 being used for:

[0127] The target model is constructed, which includes multiple physical quantities to be calculated and multiple known physical quantities. The multiple physical quantities to be calculated include at least historical perturbation forces and historical perturbation moments.

[0128] The historical disturbance force and the historical disturbance torque are calculated based on the known physical quantities and the target model.

[0129] The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, and the disturbance formula.

[0130] Optionally, the known physical quantities include historical applied force, historical applied torque, and historical twist angular velocity;

[0131] The device further includes a second acquisition unit, the second acquisition unit being configured to:

[0132] The historical applied force and the historical applied torque are obtained from the force / torque sensor;

[0133] The historical twist angular velocity is obtained from the gyroscope;

[0134] The third computing unit 250 is used for:

[0135] The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, the historical twist angular velocity, and the disturbance formula.

[0136] Optionally, the known physical quantities include historical insertion velocity, historical needle tip force, and historical axial friction force;

[0137] The device further includes a third acquisition unit, the third acquisition unit being configured to:

[0138] The historical insertion velocity is obtained from the accelerometer;

[0139] The historical needle tip force was calculated based on the historical insertion velocity and the needle tip force formula.

[0140] The historical axial friction force was calculated based on the historical needle tip force and axial friction force formulas.

[0141] The third computing unit 250 is used for:

[0142] The historical disturbance force and the historical disturbance torque are calculated based on the historical needle tip force, the historical axial friction force, the historical twisting angular velocity, the historical applied force, the historical applied torque, and the target model.

[0143] Optionally, the needle tip force formula includes multiple coefficients and multiple physical quantities to be detected. The multiple coefficients include the elastic coefficient of the skin, the viscosity coefficient of the skin, the elastic coefficient of the muscle, and the viscosity coefficient of the muscle. The multiple physical quantities to be detected include the insertion depth of the skin layer, the basic deformation distance of the skin, the insertion depth of the muscle layer, and the tissue deformation.

[0144] Optionally, the first computing unit 220 is used for:

[0145] The desired disturbance force and desired disturbance torque are calculated based on the desired twist angular velocity and the disturbance formula, and the actual disturbance force and actual disturbance torque are calculated based on the actual twist angular velocity and the actual insertion velocity.

[0146] The disturbance force difference is obtained based on the expected disturbance force and the actual disturbance force, and the disturbance torque difference is obtained based on the expected disturbance torque and the actual disturbance torque.

[0147] The second computing unit 230 is used for:

[0148] The desired applied force and desired applied torque of the needle-piercing robot are calculated based on the desired twisting angular velocity, the desired insertion velocity, the desired disturbance force and the desired disturbance torque, and the target model.

[0149] The actual applied force and actual applied torque are obtained from the force / torque sensor;

[0150] The difference between the applied force and the actual applied force is obtained based on the expected applied force and the actual applied force; the difference between the applied torque and the expected applied torque is obtained based on the actual applied torque.

[0151] The control unit 240 is used for:

[0152] The actual applied force is adjusted to the desired applied force based on the applied force difference, and the actual applied torque is adjusted to the desired applied force based on the applied torque difference, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

[0153] Based on the acupuncture robot control method provided in the above embodiments, this application also provides an acupuncture robot control device, which includes:

[0154] The processor and memory may be present, and the number of processors may be one or more. In some embodiments of this application, the processor and memory may be connected via a bus or other means.

[0155] Memory may include read-only memory and random access memory, and provides instructions and data to the processor. A portion of the memory may also include NVRAM. Memory stores operating devices and operating instructions, executable modules or data structures, or subsets thereof, or extended sets thereof, wherein operating instructions may include various operating instructions for implementing various operations. Operating devices may include various device programs for implementing various basic services and processing hardware-based tasks.

[0156] The processor controls the operation of the terminal device; the processor can also be called the CPU.

[0157] The methods disclosed in the embodiments of this application can be applied to a processor or implemented by a processor. A processor can be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above method can be completed by integrated logic circuits in the processor's hardware or by instructions in software form. The processor can be a general-purpose processor, DSP, ASIC, FPGA, or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this application. A general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this application can be directly embodied as execution by a hardware decoding processor, or as a combination of hardware and software modules in the decoding processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. This storage medium is located in memory; the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method.

[0158] This application also provides a computer-readable storage medium for storing program code that is used to execute any of the methods in the foregoing embodiments.

[0159] In the context of this application, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution apparatus, device, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0160] It should be noted that the computer-readable medium described above in this application can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution device, apparatus, or device. In this application, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium may be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution device, apparatus, or apparatus. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wires, optical fibers, RF (radio frequency), etc., or any suitable combination thereof.

[0161] When describing elements of various embodiments of this application, the articles “a,” “an,” “this,” and “described” are all intended to indicate that there are one or more elements. The words “comprising,” “including,” and “having” are inclusive and mean that there may be other elements in addition to those listed.

[0162] It should be noted that those skilled in the art will understand that all or part of the processes in the above method embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes described in the above method embodiments. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0163] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof. These programming languages ​​include, but are not limited to, object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0164] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the apparatus embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0165] The above description is merely a preferred embodiment of this application. Although this application has disclosed preferred embodiments above, it is not intended to limit this application. Any person skilled in the art can make many possible variations and modifications to the technical solutions of this application using the methods and techniques disclosed above, or modify them into equivalent embodiments with equivalent changes, without departing from the scope of the technical solutions of this application. Therefore, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of this application without departing from the content of the technical solutions of this application shall still fall within the protection scope of the technical solutions of this application.

Claims

1. A control device for an acupuncture robot, characterized in that, include: The first acquisition unit is used to acquire the difference between the expected and actual twisting angular velocities of the acupuncture robot when performing traditional Chinese medicine acupuncture, as well as the difference between the expected and actual insertion speeds. The first calculation unit is used to calculate the difference in disturbance force and the difference in disturbance torque based on the difference in twist angular velocity and the disturbance formula. The disturbance formula includes static unbalance and inertia matrix. The static unbalance is the vector from the geometric center of the rotating part in the end clamping platform to the center of mass of the rotating part. The inertia matrix is ​​the inertia of the rotating part relative to the geometric center to the center of mass. The perturbation formula satisfies: in, The disturbance force generated by static imbalance The disturbance torque generated by dynamic imbalance The mass of the rotating part, The rotational angular velocity, The static imbalance quantity is... For the inertia matrix, The cross product matrix of the twist angular velocity; The second calculation unit is used to calculate the applied force difference and applied torque difference of the needle-piercing robot based on the twisting angular velocity difference, the insertion velocity difference, the disturbance force difference and the disturbance torque difference, and the target model. The target model is the kinematic and dynamic model of the end-effector platform of the needle-piercing robot. The control unit is used to adjust the actual applied force and the actual applied torque according to the difference between the applied force and the difference between the applied torque, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

2. The apparatus according to claim 1, characterized in that, The device further includes a third computing unit, the third computing unit being used for: The target model is constructed, which includes multiple physical quantities to be calculated and multiple known physical quantities. The multiple physical quantities to be calculated include at least historical perturbation forces and historical perturbation moments. The historical disturbance force and the historical disturbance torque are calculated based on the known physical quantities and the target model. The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, and the disturbance formula.

3. The apparatus according to claim 2, characterized in that, The known physical quantities include historical applied force, historical applied torque, and historical twist angular velocity; The device further includes a second acquisition unit, the second acquisition unit being configured to: The historical applied force and the historical applied torque are obtained from the force / torque sensor; The historical twist angular velocity is obtained from the gyroscope; The third computing unit is used for: The static unbalance and the inertia matrix are calculated based on the historical disturbance force, the historical disturbance torque, the historical twist angular velocity, and the disturbance formula.

4. The apparatus according to claim 3, characterized in that, Several of the known physical quantities include historical insertion velocity, historical needle tip force, and historical axial friction force; The device further includes a third acquisition unit, the third acquisition unit being configured to: The historical insertion velocity is obtained from the accelerometer; The historical needle tip force was calculated based on the historical insertion velocity and the needle tip force formula. The historical axial friction force was calculated based on the historical needle tip force and axial friction force formulas. The third computing unit is used for: The historical disturbance force and the historical disturbance torque are calculated based on the historical needle tip force, the historical axial friction force, the historical twisting angular velocity, the historical applied force, the historical applied torque, and the target model.

5. The apparatus according to claim 4, characterized in that, The needle tip force formula includes multiple coefficients and multiple physical quantities to be detected. The multiple coefficients include the elastic coefficient of the skin, the viscosity coefficient of the skin, the elastic coefficient of the muscle, and the viscosity coefficient of the muscle. The multiple physical quantities to be detected include the insertion depth of the skin layer, the basic deformation distance of the skin, the insertion depth of the muscle layer, and the tissue deformation.

6. The apparatus according to any one of claims 1-5, characterized in that, The first computing unit is used for: The desired disturbance force and desired disturbance torque are calculated based on the desired twist angular velocity and the disturbance formula, and the actual disturbance force and actual disturbance torque are calculated based on the actual twist angular velocity and the actual insertion velocity. The disturbance force difference is obtained based on the expected disturbance force and the actual disturbance force, and the disturbance torque difference is obtained based on the expected disturbance torque and the actual disturbance torque. The second computing unit is used for: The desired applied force and desired applied torque of the needle-piercing robot are calculated based on the desired twisting angular velocity, the desired insertion velocity, the desired disturbance force and the desired disturbance torque, and the target model. The actual applied force and actual applied torque are obtained from the force / torque sensor; The difference between the applied force and the actual applied force is obtained based on the expected applied force and the actual applied force; the difference between the applied torque and the expected applied torque is obtained based on the actual applied torque. The control unit is used for: The actual applied force is adjusted to the desired applied force based on the applied force difference, and the actual applied torque is adjusted to the desired applied torque based on the applied torque difference, so as to control the actual rotational angular velocity of the acupuncture robot to be the desired rotational angular velocity and the actual insertion speed to be the desired insertion speed when performing traditional Chinese medicine acupuncture.

Images