Impedance control method and system of acupuncture robot

Abstract

The invention discloses an impedance control method and system of an acupuncture robot, and relates to the technical field of automatic control, and the method comprises the steps: positioning a mark point to be needled; constructing a variable proportion impedance control model based on force differential; determining a kinetic equation of an execution mechanism of the acupuncture robot according to the variable proportion impedance control model; according to the positioning of the mark points, the given needle inserting depth and the movement time, the movement track of the acupuncture robot is calculated; and controlling the acupuncture robot to perform acupuncture on the mark points according to the motion trail and a kinetic equation of the execution mechanism. According to the variable-proportion impedance control model based on the force differential, the control stability and accuracy of the acupuncture robot are improved, and therefore the flexibility and safety during acupuncture are improved.

Description

Technical Field

[0001] The present application relates to the field of automatic control technology, and in particular to an impedance control method and system for an acupuncture robot. Background Art

[0002] Current research on acupuncture robots primarily focuses on using computer positioning technology to identify acupoints and control the robots to mimic the practitioner's techniques while ensuring safety during the acupuncture process. In recent years, automated acupoint positioning technology has been widely adopted in medical robots such as puncture robots, moxibustion robots, and massage robots. Some surgical robots and surgical assistance robots have demonstrated high technical performance in path planning and precision manipulation. However, achieving the "qi" or "needle sensation" state during acupuncture remains a questionable standard in traditional Chinese medicine, requiring robots to possess multi-dimensional, precise, and sensitive force sensing capabilities. Despite some progress in positioning tracking, path planning, and precision manipulation, an intelligent acupuncture robot system capable of performing needle manipulation, twisting, and changing needles remains lacking.

[0003] During robot operation, due to the huge difference between the mechanical structure and the physiological structure of the human body, it is far from enough to only pursue the tracking accuracy of the trajectory without ensuring the safety of the control process. Summary of the Invention

[0004] The main purpose of the embodiments of the present application is to provide an impedance control method and system for an acupuncture robot to improve the flexibility and safety of the acupuncture robot during acupuncture.

[0005] To achieve the above objectives, one aspect of an embodiment of the present application provides an impedance control method for an acupuncture robot, the method comprising the following steps:

[0006] Locate the marked points to be acupunctured;

[0007] Construct a variable proportional impedance control model based on force differentiation;

[0008] Determining the dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model;

[0009] Calculating the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time;

[0010] The acupuncture robot is controlled to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

[0011] In some embodiments, the method of locating the marked point to be acupunctured comprises the following steps:

[0012] The RGB depth image obtained by the RGBD camera is used to jointly calibrate RGB and depth, and then the extrinsic parameter matrix of the RGBD camera is calculated;

[0013] Measuring the actual depth values ​​of the calibration plate at different distances, comparing the values ​​with the depth readings of the RGBD camera, constructing a depth error model, and using a nonlinear optimization algorithm to correct the depth error of the RGBD camera based on the depth error model to obtain an optimized parameter matrix;

[0014] Converting the RGB image in the RGB depth image to the HSV color space, and identifying the marked points in the RGB image as regions of interest using a Hough ellipse fitting method;

[0015] According to the RGB depth image, converting the region of interest into three-dimensional coordinates in the coordinate system of the RGBD camera using the optimized parameter matrix;

[0016] Then, the pose of the marker point in the coordinate system of the RGBD camera is obtained.

[0017] In some embodiments, the method further comprises the following steps:

[0018] constructing a kinematic model for the acupuncture robot;

[0019] Determine a first homogeneous transformation matrix of the marker point in the coordinate system of the RGBD camera according to the pose;

[0020] Substituting the first homogeneous transformation matrix into the kinematic model of the acupuncture robot to obtain a second homogeneous transformation matrix;

[0021] Obtaining a fourth homogeneous transformation matrix of the actuator in the base system according to the second homogeneous transformation matrix and the third homogeneous transformation matrix between the joints of the acupuncture robot;

[0022] The joint angles of the acupuncture robot when acupuncturing the marked points are obtained by inverse kinematics according to the fourth homogeneous transformation matrix.

[0023] In some embodiments, constructing a kinematic model for the acupuncture robot comprises the following steps:

[0024] The first dynamic equation established for the actuator of the acupuncture robot is:

[0025]

[0026] Among them, P is the lead parameter, m i is the mass of the connecting rod of the i-th joint, I i,zzis the moment of inertia of the i-th joint on the z-axis.

[0027] In some embodiments, constructing a variable proportional impedance control model based on force differential includes the following steps:

[0028] The variable proportional impedance control model based on force differential is constructed as follows:

[0029]

[0030] Among them, M, B, They represent the mass coefficient, damping coefficient and stiffness matrix of the acupuncture robot respectively, E is the deviation between the actual position and the expected position, E = X d -X r , where X d is the actual position of the robot, X r is the desired position of the robot, E, X d 、 is the contact force; α<0 is the gain coefficient of the contact force differential, ReLU(x) is the linear rectification function, ReLU(x)=max(0,x), sgn(x) is the sign function,

[0031] In some embodiments, determining the dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model includes the following steps:

[0032] The second dynamic equation of the actuator of the acupuncture robot is determined according to the variable proportional impedance control model as follows:

[0033]

[0034] Where Λ(x)=J -T M(q)J -1 is the inertia matrix in Cartesian space, is the Coriolis force and centrifugal force matrix in Cartesian space, p(x) = J -T g(q) is the gravity term in Cartesian space, F = J -T τ is the equivalent force of the actuator's control force in the Cartesian coordinate system,

[0035] In some embodiments, controlling the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator includes the following steps:

[0036] Solving the equivalent force of the control force of the actuator in a Cartesian coordinate system according to the dynamic equation of the actuator;

[0037] The first dynamic equation of the actuator of the acupuncture robot is substituted into the second dynamic equation to obtain the inertia matrix:

[0038] Substituting the second dynamic equation into the variable proportional impedance control model, the equivalent force is obtained as follows:

[0039]

[0040] The acupuncture robot is controlled to perform acupuncture on the marked point according to the motion trajectory and the equivalent force of the actuator.

[0041] To achieve the above objectives, another aspect of the present application provides an impedance control system for an acupuncture robot, the system comprising:

[0042] Marking positioning module, used to locate the marking points to be acupunctured;

[0043] Model building module, used to build a variable proportional impedance control model based on force differentiation;

[0044] an equation determination module, configured to determine a dynamic equation of an actuator of the acupuncture robot according to the variable proportional impedance control model;

[0045] A trajectory calculation module is used to calculate the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time;

[0046] An acupuncture control module is used to control the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

[0047] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application provides an electronic device, which includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the above-mentioned method when executing the computer program.

[0048] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program implements the above-mentioned method when executed by a processor.

[0049] The embodiments of the present application include at least the following beneficial effects:

[0050] This application can locate the marked points for acupuncture; construct a variable proportional impedance control model based on force differentials; determine the dynamic equations of the acupuncture robot's actuator based on the variable proportional impedance control model; calculate the acupuncture robot's motion trajectory based on the location of the marked points, a given needle insertion depth, and motion time; and control the acupuncture robot to perform acupuncture at the marked points based on the motion trajectory and the dynamic equations of the actuator. This application, based on the variable proportional impedance control model based on force differentials, improves the control stability and accuracy of the acupuncture robot, thereby enhancing the compliance and safety during acupuncture. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0052] Figure 1 A schematic flow chart of an impedance control method for an acupuncture robot provided in an embodiment of the present application;

[0053] Figure 2 An example diagram of the coordinate relationship of the acupuncture robot provided in an embodiment of the present application;

[0054] Figure 3 This is a block diagram of the variable proportional impedance control of the acupuncture robot provided in an embodiment of the present application;

[0055] Figure 4 A schematic structural diagram of an impedance control device for an acupuncture robot provided in an embodiment of the present application;

[0056] Figure 5 A schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0057] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are merely examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the appended claims.

[0058] It will be understood that the terms "first", "second", etc. used in this application may be used herein to describe various concepts, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another. For example, without departing from the scope of the embodiments of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0059] The terms "at least one", "plurality", "each", "any", etc. used in this application include "at least one", "two" or more, "plurality" or "each", "any" or "any one", "each" or "any one" as used herein.

[0060] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which this application pertains. The terms used herein are for the purpose of describing the embodiments of this application only and are not intended to limit this application.

[0061] Reference Figure 1 The embodiment of the present application provides an impedance control method for an acupuncture robot. The method may include but is not limited to S100 to S140, as follows:

[0062] S100: Locating the marking point to be acupunctured;

[0063] S110: Construct a variable proportional impedance control model based on force differential;

[0064] S120: Determine a dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model;

[0065] S130: Calculating the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time;

[0066] S140: Controlling the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

[0067] Optionally, positioning the marked point to be acupunctured comprises the following steps:

[0068] The RGB depth image obtained by the RGBD camera is used to jointly calibrate RGB and depth, and then the extrinsic parameter matrix of the RGBD camera is calculated;

[0069] Measuring the actual depth values ​​of the calibration plate at different distances, comparing the values ​​with the depth readings of the RGBD camera, constructing a depth error model, and using a nonlinear optimization algorithm to correct the depth error of the RGBD camera based on the depth error model to obtain an optimized parameter matrix;

[0070] Converting the RGB image in the RGB depth image to the HSV color space, and identifying the marked points in the RGB image as regions of interest using a Hough ellipse fitting method;

[0071] According to the RGB depth image, converting the region of interest into three-dimensional coordinates in the coordinate system of the RGBD camera using the optimized parameter matrix;

[0072] Then, the pose of the marker point in the coordinate system of the RGBD camera is obtained.

[0073] Optionally, the method further comprises the following steps:

[0074] constructing a kinematic model for the acupuncture robot;

[0075] Determine a first homogeneous transformation matrix of the marker point in the coordinate system of the RGBD camera according to the pose;

[0076] Substituting the first homogeneous transformation matrix into the kinematic model of the acupuncture robot to obtain a second homogeneous transformation matrix;

[0077] Obtaining a fourth homogeneous transformation matrix of the actuator in the base system according to the second homogeneous transformation matrix and the third homogeneous transformation matrix between the joints of the acupuncture robot;

[0078] The joint angles of the acupuncture robot when acupuncturing the marked points are obtained by inverse kinematics according to the fourth homogeneous transformation matrix.

[0079] Optionally, constructing a kinematic model for the acupuncture robot comprises the following steps:

[0080] The first dynamic equation established for the actuator of the acupuncture robot is:

[0081]

[0082] Among them, P is the lead parameter, m i is the mass of the connecting rod of the i-th joint, I i,zz is the moment of inertia of the i-th joint on the z-axis.

[0083] Optionally, the constructing of a variable proportional impedance control model based on force differential comprises the following steps:

[0084] The variable proportional impedance control model based on force differential is constructed as follows:

[0085]

[0086] Among them, M, B, They represent the mass coefficient, damping coefficient and stiffness matrix of the acupuncture robot respectively, E is the deviation between the actual position and the expected position, E = X d -X r , where X d is the actual position of the robot, X r is the desired position of the robot, E, X d 、 is the contact force; α<0 is the gain coefficient of the contact force differential, ReLU(x) is the linear rectification function, ReLU(x)=max(0,x), sgn(x) is the sign function,

[0087] Optionally, determining the dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model includes the following steps:

[0088] The second dynamic equation of the actuator of the acupuncture robot is determined according to the variable proportional impedance control model as follows:

[0089]

[0090] Where Λ(x)=J -T M(q)J -1 is the inertia matrix in Cartesian space, is the Coriolis force and centrifugal force matrix in Cartesian space, p(x) = J -T g(q) is the gravity term in Cartesian space, F = J -T τ is the equivalent force of the actuator's control force in the Cartesian coordinate system,

[0091] Optionally, controlling the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator comprises the following steps:

[0092] Solving the equivalent force of the control force of the actuator in a Cartesian coordinate system according to the dynamic equation of the actuator;

[0093] The first dynamic equation of the actuator of the acupuncture robot is substituted into the second dynamic equation to obtain the inertia matrix:

[0094] Substituting the second dynamic equation into the variable proportional impedance control model, the equivalent force is obtained as follows:

[0095]

[0096] The acupuncture robot is controlled to perform acupuncture on the marked point according to the motion trajectory and the equivalent force of the actuator.

[0097] Next, the solution of the embodiment of the present application will be introduced and explained in detail with reference to specific application examples.

[0098] How to ensure the flexibility and safety of human-computer interaction during the acupuncture process is a major challenge in the application of acupuncture robots. In fact, during the process of the acupuncture robot operating the target, there is a coupling relationship between the movement of the acupuncture robot and the reaction force it exerts on the environment. However, although MPC can handle multivariable systems, it poses a huge challenge to computing performance, and the performance of MPC depends on the accuracy of the system model. The use of an impedance control model can effectively achieve a dynamic balance between trajectory tracking and contact force. The solution of this embodiment is combined with sensors and encoders with high-frequency response characteristics to provide a higher sampling frequency, and is combined with a control model with less computational effort to increase the system control frequency.

[0099] During acupuncture, how to control force is the core of the acupuncture task.

[0100] To address the above issues, this embodiment proposes a variable proportional impedance control method suitable for human acupuncture contact scenarios based on the principle of adaptive impedance control, which autonomously adjusts the dynamic balance process between trajectory tracking and contact force according to the changes in transient force. The technical solutions of this embodiment include:

[0101] 1. Kinematic modeling and acupoint positioning of acupuncture robot.

[0102] Kinematic modeling:

[0103] The coordinate relationship of the acupuncture robot is as follows Figure 2 As shown, {0} and {e} represent the base coordinate system and the end effector coordinate system of the acupuncture robot arm, respectively, {ee} represents the needle tip coordinate system, {gc} represents the global camera coordinate system, and {gat} is the acupuncture point coordinate system. The closed chain is:

[0104] 0 T e e T ee ee T gat = 0 T gc gc T gat (1)

[0105] in e T ee represents the homogeneous transformation matrix of the needle tip under the end effector, 0 T gc Represents the homogeneous transformation matrix of the global camera coordinate system in the base coordinate system, 0 T e Represents the homogeneous transformation matrix of the end of the acupuncture robot arm under the base system, gc T gat Represents the homogeneous transformation matrix of the acupoint landmark in the camera system, ee T gat Represents the homogeneous transformation matrix of acupoint landmarks under the needle tip system

[0106] According to formula (1), the kinematic equation of the end under the base is:

[0107]

[0108] The 8-DOF acupuncture robot consists of a 6-DOF industrial robot arm and a 2-DOF end effector. The dynamic equation of the 2-DOF end effector can be described as:

[0109]

[0110] Where P is the lead parameter, m i is the mass of the connecting rod of the i-th joint, I i,zz is the moment of inertia of the i-th joint on the z-axis.

[0111] Acupoint positioning method based on cooperative landmarks:

[0112] An RGB-D camera is used to capture target images of specific acupuncture points marked by doctors. The three-dimensional position and posture of the collaborative markers are accurately calculated by combining color gamut screening, image processing, geometric fitting, and point cloud data fitting.

[0113] Before positioning, the camera was calibrated using a joint calibration method combined with nonlinear depth correction. The intrinsic parameter of Depth was obtained from the checkerboard image. Its extrinsic parameter matrix was calculated by jointly calibrating RGB and depth. To improve accuracy, the nonlinear error of the depth camera was optimized. The actual depth values ​​of the calibration plate at different distances were measured and compared with the depth sensor readings. A depth error model was constructed, and the sensor's depth error was corrected using a nonlinear optimization algorithm. The optimized parameter matrix is:

[0114]

[0115] E x = {Ri n ,ti n} (10)

[0116] Among them, f x and f y are the focal length of the camera in the x / y direction, c x and c y is the camera's x / y offset, R in Represents the rotation matrix between the RGB camera and the depth camera, t in Represents the translation vector between the RGB camera and the depth camera.

[0117] Convert the RGB image to HSV color space and extract the region of interest through the mask image. The elliptical mark in the image can be identified by the Hough ellipse fitting method. According to the acquired RGB and depth images, the two-dimensional image of the mark is converted into three-dimensional coordinates in the camera coordinate system through the camera intrinsic parameters. Assume that the pixel coordinates of the cooperation mark are (x img ,y img ), the depth value of this point in the depth map is z depth , so the three-dimensional coordinates P of the target marker in the camera coordinate system C It can be expressed as:

[0118]

[0119] Z C =z depth (13)

[0120] The depth map corresponding to the marked area is converted into a 3D point cloud, so that the 2-D pixels in the image form a one-to-one mapping with the 3-D points in the 3D space. According to the plane fitting method, the sampling consistency random sampling algorithm is used for plane fitting, and the normal vector of the plane where the cooperation mark is located can be solved as:

[0121] ax+by+cz+d=0 (14)

[0122] According to formula (14), the normal vector of the plane where the marker point is located is

[0123] Combining (11) to (14) we can get the pose of the cooperative landmarks, given the reference direction is the X-axis of the target coordinate system, and Then, the rotation matrix gc R gat and translation vectors gc t gat It can be expressed as:

[0124]

[0125] gc t gat =PC =(X C ,Y C ,Z C ) (16)

[0126] Therefore, the homogeneous transformation matrix of the cooperative landmark point in the global camera system is gc T gat for:

[0127]

[0128] Will ee T gat Substituting into formula (2), the homogeneous transformation matrix from the base system to the end system of the manipulator can be obtained as 0 T e .

[0129] On the other hand, the homogeneous transformation matrix from the i-1th joint to the i-th joint of the acupuncture robot is:

[0130]

[0131] Among them, θ i ,α i ,a i ,d i is the DH parameter.

[0132] According to the chain rule, the homogeneous transformation matrix of the robot end system under the base system is: 0 T e = 0 T1 1 T2... n- 2 T n-1 n-1 T e , each joint angle θ can be obtained through inverse kinematics i .

[0133] 2. Variable proportional impedance control scheme of acupuncture robot.

[0134] Due to inertia and the elastic structure of the skin, the robot's end-of-needle mechanism often generates large transient forces when it comes into contact with human tissue. In actual acupuncture, rapid needle insertion is required. Failure to promptly suppress transient forces can compromise the safety of the insertion process. Using traditional PID methods to control these transient forces can cause structural vibration, leading to system instability and large position control errors. Impedance control exhibits excellent compliance, enabling fast and stable motion control. It also ensures precise force control when the robot interacts with its environment, improving mission safety.

[0135] Variable proportional impedance control during soft tissue acupuncture:

[0136] The impedance control model of the acupuncture robot can be expressed as:

[0137]

[0138] Among them, M, B, They represent the mass coefficient, damping coefficient and stiffness matrix of the robot respectively, E is the deviation between the actual position and the expected position, E = X d -X r , where X d is the actual position of the robot, X r is the desired position of the robot, E, X d 、 is the contact force.

[0139] To effectively improve the response to changes in contact force, this embodiment adds a force differential term to Equation (19) for contact force compensation. However, adding the force differential term to the contact force term results in a proportional differential. This method separates force differential compensation from contact force, resulting in excessive compensation for smaller contact forces and insufficient compensation for larger contact forces. To address this issue, this paper proposes a variable proportional impedance control method that uses the force differential term as a contact force weight and can filter the direction of the force differential, improving the system's compliance and safety.

[0140] Based on this, formula (19) can be further expressed as:

[0141]

[0142] Where α<0 is the gain coefficient of the contact force differential, ReLU(x) is the linear rectification function, ReLU(x)=max(0,x), sgn(x) is the sign function,

[0143] Equation (20) adds the perception of contact force changes based on the impedance model. By adding the contact force differential as part of the response environment force weight and filtering the force differential direction through the ReLU function, it is ensured that the force differential weight satisfies: Right now: Ensure that the added force differential weight only has the effect of suppressing the rapid growth of contact force, suppressing the growth of transient force, making the control process smoother, and improving the safety of the acupuncture process.

[0144] make Substituting it into formula (20) we can get:

[0145]

[0146] Define state variable E1=E, Then the system matrix is:

[0147]

[0148] Let det(A-λI)=0, and the characteristic equation is:

[0149]

[0150] Therefore, the solution of formula (23) can be expressed as:

[0151]

[0152] When M 、 B 、 When K is positive, the real parts of all eigenvalues ​​are negative, indicating that the system is asymptotically stable. but tends to be stable; if because Then in When it tends to be stable, F e The growth of (t) is suppressed.

[0153] A complete stability proof of this system is given in Appendix A.

[0154] The environmental model of the acupuncture robot can be expressed as:

[0155]

[0156] In formula (25), k e represents the environmental stiffness coefficient, x e represents the environmental position, k e 、x e 、x r K e 、E e 、E r The one-dimensional representation of F er Represents the contact force of the actual environment.

[0157] Since the impedance control of the acupuncture actuator in any direction in Cartesian space is independent, only considering the impedance control in a specific direction, the model can be simplified as follows:

[0158]

[0159] Among them, M, B, K, F e For M, B, K, F e One-dimensional representation of .

[0160] Since only the steady state is considered, are all 0, substitute it into E=x d -x r And formula (25), we can get:

[0161] K(x d -x r )=k e (x d -x e ) (27)

[0162] Among them, xd, x r For X d 、X r One-dimensional representation of .

[0163] The steady-state error of the contact force is F e -F er , which can be expressed as Bring it into F er =k e (x r -x e ) can be obtained:

[0164]

[0165] Formula (28) can be organized as:

[0166]

[0167] From formula (29), we can see that It is a combination of system stiffness, environmental stiffness, desired position and environmental position. This means that when the environmental position x e and the environmental stiffness k e When the contact force error is known, the contact force error can be obtained by the expected position x d It can be calculated by the system stiffness K. Therefore, choosing an appropriate system stiffness K can reduce the contact force error.

[0168] Variable Proportional Impedance Control of Acupuncture Robot:

[0169] The dynamics of the 2-DOF acupuncture actuator can be expressed as:

[0170]

[0171] Where Λ(x)=J -T M(q)J -1 is the inertia matrix in Cartesian space, is the Coriolis force and centrifugal force matrix in Cartesian space, p(x) = J -T g(q) is the gravity term in Cartesian space, F = J -Tτ is the equivalent force of the actuator's control force in the Cartesian coordinate system,

[0172] Substituting (4) to (7) into equation (30), we can obtain the inertia matrix:

[0173] Substituting formula (30) into formula (20) yields:

[0174]

[0175] The block diagram of variable proportional impedance control based on the dynamic model of acupuncture robot is as follows: Figure 3 As shown in the figure, when the system is stable, the contact force between the needle tip and the environment remains constant. The model can be reduced to model-based impedance control, which reduces system complexity. As the needle tip interacts with the environment, the designed variable proportional impedance system presents dynamically changing contact forces in real time and adjusts the contact force response accordingly, suppressing transient force growth and making the motion process more compliant.

[0176] By calculating the motion trajectory based on the given needle insertion depth and motion time, the system simulates acupuncture techniques to determine the needle movement trajectory and inserts it into the skin mold. The given target trajectory is:

[0177]

[0178] Among them, T a 、T c 、T d They represent acceleration time, uniform speed time and deceleration time respectively, T m is the time proportional coefficient.

[0179] set up For the convenience of representation, let T b =T a +T c ,have:

[0180]

[0181] In order to test and compare the performance of the proposed method, the following performance index parameters are set:

[0182] Rate of change of force:

[0183]

[0184] in, The lower it is, the smaller the change in environmental force generated by the contact between the end effector and the environment during work, and the smoother the working process.

[0185] Trajectory following error:

[0186]

[0187] Among them, ε track The lower the value, the closer the actual trajectory is to the expected trajectory.

[0188] The work done by the end effector on the outside world:

[0189]

[0190] in, The smaller it is, the less energy the end effector applies during the work process.

[0191] The goal of the acupuncture task is to improve the flexibility of the working process while following a given trajectory. The smaller the above-mentioned indicators are, the better.

[0192] in conclusion:

[0193] A kinematic model of an acupuncture robot was derived, and acupoint markers were automatically located. Furthermore, a variable proportional impedance control method for the acupuncture robot was proposed, accounting for contact force variations. Needle puncture and insertion experiments on simulated silicone skin verified the effectiveness and accuracy of the proposed method. Future work will consider combining acupuncture trajectory generation with the variable proportional impedance model to further improve control accuracy.

[0194] Appendix A: System stability proof:

[0195] After the force differential term is introduced into Equation (20) as part of the environmental force weight, the system becomes a nonlinear system. Then equation (20) degenerates into the standard impedance model of equation (19). The stability of this system has been proven in many literatures. The stability of the situation was proved.

[0196] Assuming that the system works near the equilibrium point, a small disturbance is introduced near the equilibrium point, E(t)=E0+δE(t) and F e (t) = F e0 +δF e (t), and substituting it into formula (20), the linearized equation is:

[0197]

[0198] At the equilibrium point, there is as well as At the same time, when F e0 ≠0, sgn(F e0 +δF e )=sgn(F e0 ), formula (26) can be further simplified as:

[0199]

[0200] For small disturbances, this embodiment ignores high-order terms, and equation (38) can be rewritten as:

[0201]

[0202]

[0203] Will And perform Laplace transform on it, and we can get:

[0204]

[0205] make So we have:

[0206]

[0207] According to formula (41), its characteristic value can be obtained as Since the gain coefficient range is α<0, the real part of the eigenvalue is negative, so the system is stable.

[0208] In summary, this embodiment proposes a variable proportional impedance control method suitable for human acupuncture contact scenarios, achieving high-precision force control under complex acupuncture technique tracking. First, a kinematic model of the acupuncture robot was established, and a method for autonomous positioning of acupoint markers was given. Second, a variable proportional impedance control method based on force differential was proposed to ensure the safety and flexibility of the human-computer interaction process. Finally, based on the designed dexterous acupuncture mechanism, needle insertion and lifting experiments were carried out. The comparison results with other control models show that the control model proposed in this embodiment has strong stability and accuracy.

[0209] Reference Figure 4 The embodiment of the present application further provides an impedance control system for an acupuncture robot, which can implement the above-mentioned impedance control method for an acupuncture robot. The system includes:

[0210] Marking positioning module, used to locate the marking points to be acupunctured;

[0211] Model building module, used to build a variable proportional impedance control model based on force differentiation;

[0212] an equation determination module, configured to determine a dynamic equation of an actuator of the acupuncture robot according to the variable proportional impedance control model;

[0213] A trajectory calculation module is used to calculate the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time;

[0214] An acupuncture control module is used to control the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

[0215] It can be understood that the contents of the above method embodiments are applicable to the present system embodiments, the functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0216] The present invention also provides an electronic device comprising a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the method of the present invention. The electronic device can be any smart terminal, such as a tablet computer or an in-vehicle computer.

[0217] It can be understood that the contents of the above method embodiments are all applicable to the embodiments of the present device, the functions specifically implemented by the embodiments of the present device are the same as those of the method of the present application, and the beneficial effects achieved are also the same as those achieved by the method of the present application.

[0218] See also Figure 5 , Figure 5 The hardware structure of an electronic device according to another embodiment is shown. The electronic device includes:

[0219] The processor 501 may be implemented as a general-purpose CPU (Central Processing Unit), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, and is configured to execute relevant programs to implement the technical solutions provided in the embodiments of the present application.

[0220] The memory 502 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). The memory 502 can store an operating system and other application programs. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program code is stored in the memory 502 and is called by the processor 501 to execute the methods of the embodiments of this application.

[0221] Input / output interface 503, used to implement information input and output;

[0222] Communication interface 504, used to implement communication interaction between this device and other devices, which can be achieved through wired means (such as USB, network cable, etc.) or wireless means (such as mobile network, WiFi, Bluetooth, etc.);

[0223] Bus 505 , which transmits information between various components of the device (e.g., processor 501 , memory 502 , input / output interface 503 , and communication interface 504 );

[0224] The processor 501 , the memory 502 , the input / output interface 503 and the communication interface 504 are connected to each other in communication within the device via a bus 505 .

[0225] An embodiment of the present application further provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the method of the present application is implemented.

[0226] It can be understood that the contents of the above method embodiments are all applicable to the present storage medium embodiment, the functions specifically implemented by the present storage medium embodiment are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0227] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely arranged relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0228] The embodiments described in the embodiments of this application are intended to more clearly illustrate the technical solutions of the embodiments of this application and do not constitute a limitation on the technical solutions provided by the embodiments of this application. Those skilled in the art will appreciate that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided in the embodiments of this application are also applicable to similar technical problems.

[0229] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of the present application, and may include more or fewer steps than shown in the figures, or a combination of certain steps, or different steps.

[0230] The system embodiment described above is merely illustrative. The modules described as separate components may or may not be physically separate, i.e., they may be located in one place or distributed across multiple network modules. Some or all of the modules may be selected based on actual needs to achieve the objectives of this embodiment.

[0231] Those skilled in the art will appreciate that all or some of the steps in the methods, systems, and functional modules / units in the devices disclosed above may be implemented as software, firmware, hardware, or appropriate combinations thereof.

[0232] The terms "first," "second," "third," "fourth," etc. (if any) in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequential sequence. It should be understood that the numbers used in this way are interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having," as well as any variations thereof, are intended to cover non-exclusive inclusions, e.g., a process, method, system, product, or apparatus comprising a series of steps or modules is not necessarily limited to those steps or modules clearly listed, but may include other steps or modules that are not clearly listed or inherent to these processes, methods, products, or apparatus.

[0233] It should be understood that in this application, "at least one (item)" means one or more, and "plurality" means two or more. "And / or" is used to describe the association relationship of associated objects, indicating that three relationships may exist. For example, "A and / or B" can mean: only A exists, only B exists, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally indicates that the previous and next associated objects are in an "or" relationship. "At least one of the following items" or similar expressions refers to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, c can be single or multiple.

[0234] In the several embodiments provided in this application, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For example, the division of the modules described above is merely a logical function division. In actual implementation, there may be other division methods, such as multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of systems or modules, which can be electrical, mechanical or other forms.

[0235] The modules described above as separate components may or may not be physically separate, and the components shown as modules may or may not be physical modules, that is, they may be located in one place or distributed across multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present embodiment.

[0236] In addition, the functional modules in the various embodiments of the present application may be integrated into a processing module, or each module may exist physically separately, or two or more modules may be integrated into a single module. The above-mentioned integrated modules may be implemented in the form of hardware or software functional modules.

[0237] If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes multiple instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk, and other media that can store programs.

[0238] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but are not intended to limit the scope of the present invention. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and essence of the present invention should be within the scope of the present invention.

Claims

1. An impedance control method for an acupuncture robot, characterized in that: The method comprises the following steps: Locate the marked points to be acupunctured; Construct a variable proportional impedance control model based on force differentiation; Determining the dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model; Calculating the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time; The acupuncture robot is controlled to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

2. The impedance control method of an acupuncture robot according to claim 1, characterized in that: The method of positioning the marked point to be acupunctured comprises the following steps: The RGB depth image obtained by the RGBD camera is used to jointly calibrate RGB and depth, and then the extrinsic parameter matrix of the RGBD camera is calculated; Measuring the actual depth values ​​of the calibration plate at different distances, comparing the values ​​with the depth readings of the RGBD camera, constructing a depth error model, and using a nonlinear optimization algorithm to correct the depth error of the RGBD camera based on the depth error model to obtain an optimized parameter matrix; Converting the RGB image in the RGB depth image to the HSV color space, and identifying the marked points in the RGB image as regions of interest using a Hough ellipse fitting method; According to the RGB depth image, converting the region of interest into three-dimensional coordinates in the coordinate system of the RGBD camera using the optimized parameter matrix; Then, the pose of the marker point in the coordinate system of the RGBD camera is obtained.

3. The impedance control method of an acupuncture robot according to claim 2, characterized in that: The method further comprises the following steps: Constructing a kinematic model for the acupuncture robot; Determine a first homogeneous transformation matrix of the marker point in the coordinate system of the RGBD camera according to the pose; Substituting the first homogeneous transformation matrix into the kinematic model of the acupuncture robot to obtain a second homogeneous transformation matrix; Obtaining a fourth homogeneous transformation matrix of the actuator in the base system according to the second homogeneous transformation matrix and the third homogeneous transformation matrix between the joints of the acupuncture robot; The joint angles of the acupuncture robot when acupuncturing the marked points are obtained by inverse kinematics according to the fourth homogeneous transformation matrix.

4. The impedance control method of an acupuncture robot according to claim 3, characterized in that: The kinematic model of the acupuncture robot is constructed, comprising the following steps: The first dynamic equation established for the actuator of the acupuncture robot is: Among them, P is the lead parameter, m i is the mass of the connecting rod of the i-th joint, I i,zz is the moment of inertia of the i-th joint on the z-axis.

5. The impedance control method of an acupuncture robot according to claim 1, characterized in that: The construction of the variable proportional impedance control model based on force differential includes the following steps: The variable proportional impedance control model based on force differential is constructed as follows: in, They represent the mass coefficient, damping coefficient and stiffness matrix of the acupuncture robot respectively, E is the deviation between the actual position and the expected position, E = X d -X r , where X d is the actual position of the robot, X r is the desired position of the robot, is the contact force; α<0 is the gain coefficient of the contact force differential, ReLU(x) is the linear rectification function, ReLU(x)=max(0,x), sgn(x) is the sign function, 6. The impedance control method of an acupuncture robot according to claim 1, characterized in that: Determining the dynamic equation of the actuator of the acupuncture robot according to the variable proportional impedance control model includes the following steps: The second dynamic equation of the actuator of the acupuncture robot is determined according to the variable proportional impedance control model as follows: Where Λ(x)=J -T M(q)J -1 is the inertia matrix in Cartesian space, is the Coriolis force and centrifugal force matrix in Cartesian space, p(x)=J -T g(q) is the gravity term in Cartesian space, F = J -T τ is the equivalent force of the actuator's control force in the Cartesian coordinate system, 7. The impedance control method of an acupuncture robot according to claim 6, characterized in that: The step of controlling the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator comprises the following steps: Solving the equivalent force of the control force of the actuator in a Cartesian coordinate system according to the dynamic equation of the actuator; The first dynamic equation of the actuator of the acupuncture robot is substituted into the second dynamic equation to obtain the inertia matrix: Substituting the second dynamic equation into the variable proportional impedance control model, the equivalent force is obtained as follows: The acupuncture robot is controlled to perform acupuncture on the marked point according to the motion trajectory and the equivalent force of the actuator.

8. An impedance control system for an acupuncture robot, characterized in that: The system comprises: Marking positioning module, used to locate the marking points to be acupunctured; Model building module, used to build a variable proportional impedance control model based on force differentiation; an equation determination module, configured to determine a dynamic equation of an actuator of the acupuncture robot according to the variable proportional impedance control model; A trajectory calculation module is used to calculate the motion trajectory of the acupuncture robot according to the positioning of the marking points, the given needle insertion depth and the motion time; An acupuncture control module is used to control the acupuncture robot to perform acupuncture on the marked point according to the motion trajectory and the dynamic equation of the actuator.

9. An electronic device, characterized in that: The electronic device includes a memory and a processor, the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images