Acupuncture robot bending correction control method, device, equipment and storage medium

Abstract

The present application discloses a control method, device, equipment and storage medium for bending correction of an acupuncture robot, and relates to the field of robot control technology. The method includes: determining the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; determining the position to be acupunctured as the target position; planning a multi-segment broken line trajectory for the path between the initial position and the target position, and controlling the movement of the acupuncture robot according to the multi-segment broken line trajectory; determining the position of the needle tip and the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtaining the external force and the length of the needle; calculating the bending angle of the needle according to the current position of the needle tip and the needle head, the external force and the length of the needle; and controlling the acupuncture robot to perform acupuncture according to the bending angle. The uncertainty of the planned trajectory can be reduced by using a multi-segment broken line trajectory; combining the position of the needle, the external force, the length and the bending angle of the needle, the bending of the needle during the acupuncture process can be corrected.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to an acupuncture robot bending correction control method, device, equipment and storage medium. Background Art

[0002] Before acupuncture, the acupuncture robot needs to consider a series of strict environmental constraints when moving from the initial posture (position and posture) to the target posture for acupuncture preparation. However, the related technology generates a trajectory from the initial position to the target position through trajectory optimization for the acupuncture robot, and the trajectory is uncertain and has poor stability. During acupuncture, the needle is easily bent by external forces, which will reduce the accuracy of acupuncture. Summary of the invention

[0003] The main purpose of the embodiments of the present application is to propose an acupuncture robot bending correction control method, device, equipment and storage medium to improve the trajectory stability of the acupuncture robot and reduce the probability of needle bending during acupuncture.

[0004] To achieve the above purpose, one aspect of an embodiment of the present application provides a bending correction control method for an acupuncture robot, the method comprising the following steps:

[0005] Determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; determine the position to be acupunctured as the target position;

[0006] Planning a plurality of broken line trajectories for a path between the initial position and the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories;

[0007] Determine the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtain the external force acting on the needle and the length of the needle;

[0008] Calculating the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle;

[0009] The acupuncture robot is controlled to perform acupuncture according to the bending angle.

[0010] In some embodiments, the step of determining the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot comprises the following steps:

[0011] Establishing kinematic equations for the acupuncture robot;

[0012] Determine the current joint angle data of the acupuncture robot according to the kinematic equation;

[0013] Determining the position of the needle tip as the initial position from the current joint angle data of the acupuncture robot;

[0014] The kinematic equation is:

[0015] 0 T n (q1,q2,L,q n )= 0 T1 1 T2L n-1 T n ;

[0016] Where q=[q1,q2,L,q n ] T is the joint angle of the acupuncture robot, {0}~{n} are the corresponding joint coordinate systems, and the origin of {n} is located at the needle tip; i T j Represents the homogeneous transformation matrix of joint coordinate system {j} relative to joint coordinate system {i}.

[0017] In some embodiments, determining the position to be acupunctured as the target position comprises the following steps:

[0018] Establishing the velocity level kinematic equation of the acupuncture robot according to the kinematic equation;

[0019] The velocity level kinematic equation is:

[0020]

[0021] in, is the terminal position of the acupuncture robot, for The derivative of for The derivative of represents the derivative of the joint angle q; J n is the Jacobian matrix from joint space to operation space; 0 p n =[p x ,p y ,p z ] T is the three-dimensional position vector of the acupuncture robot end in the joint coordinate system {0}, p x , p y , p z Represent the coordinate components of the x, y, and z axes respectively; is the posture of the end of the acupuncture robot expressed in Euler angles, α, β, and γ represent the roll, pitch, and yaw components of the Euler angles, respectively;

[0022] The joint angular velocity equation of the acupuncture robot is determined according to the velocity level kinematic equation as follows:

[0023]

[0024] in, is the generalized inverse matrix of the acupuncture robot;

[0025] When the local camera disposed at the end of the acupuncture robot detects the target position, the local coordinates of the target position in the local camera coordinate system are determined; according to the local coordinates and the velocity level kinematic equation, the end position of the acupuncture robot is determined as the position of the target position;

[0026] The end position of the acupuncture robot in the local camera coordinate system is:

[0027]

[0028] in, is the terminal position of the acupuncture robot at time k, is the local camera coordinate system {L C} is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the local coordinate, is the terminal posture of the acupuncture robot at time k;

[0029] When the local camera fails to detect the target position, the global coordinates of the target position in the global camera coordinate system are determined; the end position and posture of the acupuncture robot are determined according to the global coordinates and the velocity level kinematic equation, and the obtained end position and posture are substituted into the joint angular velocity equation, and the substitution result is integrated, and the integrated result is used as the position and posture of the target position in the global camera coordinate system;

[0030] The end position of the acupuncture robot in the global camera coordinate system is:

[0031]

[0032] in, is the terminal position of the acupuncture robot at time k, The global camera coordinate system {G C} is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the global coordinate.

[0033] In some embodiments, planning a multi-segment broken line trajectory for the path from the initial position to the target position includes the following steps:

[0034] Establishing a discrete joint space model at time k for the acupuncture robot;

[0035] The discrete joint space model is:

[0036] q k+1 =Aq k +Bu k ;

[0037] in, represents the joint angles of the acupuncture robot, represents the unit state matrix, represents the control matrix, T s represents the discrete time interval, u represents the joint angular velocity of the acupuncture robot, u is the control quantity,

[0038] The augmented dynamic equation of the discrete joint space model is:

[0039]

[0040] in, is the preset reference joint angle, is the target joint angle, is the tracking error between the actual joint angle and the reference joint angle at time k, and the state quantity of the discrete joint space model is

[0041] Establishing a piecewise constant tracking model based on the discrete joint space model;

[0042] The tracking model is:

[0043]

[0044] Among them, Lv is the first objective function, is the incremental control sequence defined on the field of view N, and is a positive definite weight matrix, is a convex polygon constraint set, represents the maximum output allowed set;

[0045] The constraints of the tracking model are:

[0046] T s J i (q i ) k+i -S di =0;

[0047] Among them, u k+i represents the control amount corresponding to the i-th field of view, J i represents the Jacobian matrix corresponding to the i-th field of view, S di is the target trajectory tracking value corresponding to the moment;

[0048] The tracking model including the constraints is:

[0049]

[0050] stvel-con;

[0051] Among them, L Sp is the second objective function, w is the trajectory tracking weight;

[0052] The tracking model including the constraint conditions is used to plan a multi-segment broken line trajectory for the path from the initial position to the target position.

[0053] In some embodiments, controlling the movement of the acupuncture robot according to the multiple segments of the broken line trajectory comprises the following steps:

[0054] Planning a first desired trajectory of a plurality of segments of the broken line trajectories;

[0055] Updating the tracking model including the constraint condition to obtain a first tracking model;

[0056] Substituting each segment of the first desired trajectory into the first tracking model, and controlling the acupuncture robot to move;

[0057] The first tracking model is:

[0058]

[0059] Among them, L SR is the third objective function, J Tk is the Jacobian matrix of the acupuncture robot with n degrees of freedom, w r is the first trajectory tracking weight, J Rxy represents the sub-Jacobian matrix corresponding to the roll and pitch degrees of freedom; q n ,q n-1 S represents the joint angle of the acupuncture actuator with 2 degrees of freedom at the end; d1 is the first desired trajectory;

[0060]

[0061] Wherein, P0 is the initial position, O is the target position, W is the right-angle turning point, and the path from the initial position to the target position is P0→W→O; λ is the planning step size, N is the control field of view, ρ is a positive integer, c is the number of planning times of the P0→W or W→O segment, and ∈ is an adjustable positive number; when ||PW||2<∈, the trajectory of P0→W is switched to W→O.

[0062] In some embodiments, the step of calculating the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle comprises the following steps:

[0063] A spherical surface is established by taking the target position as the center of the sphere and the length of the needle as the radius;

[0064] Determining an expected position on the spherical surface, and determining the coordinates of the expected position according to the current position of the needle tip, the current position of the needle head, the external force and the radius;

[0065] The bending angle of the needle is calculated using an arc cosine function according to the coordinates of the current position of the needle tip, the coordinates of the current position of the needle head, and the coordinates of the expected position.

[0066] In some embodiments, controlling the acupuncture robot to perform acupuncture according to the bending angle comprises the following steps:

[0067] Planning the expected position of the needle tip at the next moment according to the bending angle;

[0068] generating a second expected trajectory according to the expected position;

[0069]

[0070] Among them, S d2 is the second expected trajectory, M is the expected position of the needle tip at the next moment; P′ is the current position of the needle tip; 0p M is the coordinate of the expected position of the needle tip at the next moment relative to the coordinate system of the needle tip; P′ is the coordinate of the current position of the needle tip relative to the coordinate system of the needle tip;

[0071] Updating the tracking model including the constraint condition to obtain a second tracking model;

[0072] The second tracking model is:

[0073]

[0074] Among them, L AR is the fourth objective function; J T 'k is the linear velocity Jacobian matrix corresponding to the needle's coordinate system {E}, w a Tracking weight for the second trajectory;

[0075] The second expected trajectory is substituted into the second tracking model, and the acupuncture robot is controlled to perform acupuncture.

[0076] To achieve the above-mentioned purpose, another aspect of the embodiment of the present application provides a bending correction control device for an acupuncture robot, the device comprising:

[0077] A position determination unit, used to determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; and determine the position to be acupunctured as the target position;

[0078] A first control unit, used for planning a plurality of broken line trajectories for a path from the initial position to the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories;

[0079] a data determination unit, for determining the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtaining the external force applied to the needle and the length of the needle;

[0080] a bending determination unit, configured to calculate the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle;

[0081] The second control unit is used to control the acupuncture robot to perform acupuncture according to the bending angle.

[0082] To achieve the above objective, another aspect of an embodiment of the present application provides an electronic device, the electronic device comprising a memory and a processor, the memory storing a computer program, and the processor implementing the above method when executing the computer program.

[0083] To achieve the above objective, 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 method when executed by a processor.

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

[0085] This embodiment can determine the position of the needle tip at the end of the acupuncture robot as the initial position based on the current joint angle data of the acupuncture robot; determine the position to be acupunctured as the target position; plan a multi-segment broken line trajectory for the path between the initial position and the target position, and control the movement of the acupuncture robot according to the multi-segment broken line trajectory; determine the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtain the external force and the length of the needle; calculate the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle; and control the acupuncture robot to perform acupuncture according to the bending angle. The path between the initial position and the target position can be relatively fixed by a multi-segment broken line trajectory, reducing the uncertainty of the planned trajectory and improving the movement stability of the acupuncture robot; combined with the position of the needle, the external force on the needle, the length of the needle and the bending angle of the needle, the bending of the needle during the acupuncture process can be corrected, thereby improving the accuracy of acupuncture. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. 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 creative work.

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

[0088] Figure 2 A schematic diagram of a curved needle provided in an embodiment of the present application;

[0089] Figure 3 An example diagram of trajectory tracking control under coarse positioning provided by an embodiment of the present application;

[0090] Figure 4 A schematic diagram of the broken line trajectory planning and tracking provided in an embodiment of the present application;

[0091] Figure 5 A trajectory tracking control diagram under precise positioning provided by an embodiment of the present application;

[0092] Figure 6 A schematic diagram of trajectory correction by spherical modeling of a curved needle provided in an embodiment of the present application;

[0093] Figure 7 A schematic diagram of the overall trajectory planning of the acupuncture robot provided in the embodiment of the present application;

[0094] Figure 8 A schematic diagram of the structure of an adaptive impedance control device for an acupuncture robot provided in an embodiment of the present application;

[0095] Fig. 9 A schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0096] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below in conjunction with 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 only examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the attached claims.

[0097] It is understood that the terms "first", "second", etc. used in this application can be used to describe various concepts in this article, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another concept. For example, without departing from the scope of the embodiment 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 can be interpreted as "at the time of" or "when" or "in response to determination".

[0098] The terms "at least one", "multiple", "each", "any", etc. used in this application, at least one includes one, two or more, multiple includes two or more, each refers to each of the corresponding multiple, and any refers to any one of the multiple.

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

[0100] Reference Figure 1 The present application embodiment provides a method for controlling bending correction of an acupuncture robot. The method may include but is not limited to steps S100 to S140, as follows:

[0101] S100: Determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; and determine the position to be acupunctured as the target position.

[0102] It is understandable that the needle can be set at the end of the acupuncture robot, the current position of the needle tip is used as the initial position, and the position that needs acupuncture is used as the target position, and the target position can be various acupuncture points.

[0103] Further, determining the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot comprises the following steps:

[0104] Establishing kinematic equations for the acupuncture robot;

[0105] Determine the current joint angle data of the acupuncture robot according to the kinematic equation;

[0106] Determining the position of the needle tip as the initial position from the current joint angle data of the acupuncture robot;

[0107] The kinematic equation is:

[0108] 0 T n (q1,q2,L,q n )= 0 T1 1 T2L n-1 T n ;

[0109] Where q=[q1,q2,L,q n ] T is the joint angle of the acupuncture robot, {0}~{n} are the corresponding joint coordinate systems, and the origin of {n} is located at the needle tip; i T j Represents the homogeneous transformation matrix of joint coordinate system {j} relative to joint coordinate system {i}.

[0110] Furthermore, determining the position to be acupunctured as the target position comprises the following steps:

[0111] Establishing the velocity level kinematic equation of the acupuncture robot according to the kinematic equation;

[0112] The velocity level kinematic equation is:

[0113]

[0114] in, is the terminal position of the acupuncture robot, for The derivative of for The derivative of represents the derivative of the joint angle q; J nis the Jacobian matrix from joint space to operation space; 0 p n =[p x ,p y ,p z ] T is the three-dimensional position vector of the acupuncture robot end in the joint coordinate system {0}, p x , p y , p z Represent the coordinate components of the x, y, and z axes respectively; is the posture of the end of the acupuncture robot expressed in Euler angles, α, β, and γ represent the roll, pitch, and yaw components of the Euler angles, respectively;

[0115] The joint angular velocity equation of the acupuncture robot is determined according to the velocity level kinematic equation as follows:

[0116]

[0117] in, is the generalized inverse matrix of the acupuncture robot;

[0118] When the local camera disposed at the end of the acupuncture robot detects the target position, the local coordinates of the target position in the local camera coordinate system are determined; according to the local coordinates and the velocity level kinematic equation, the end position of the acupuncture robot is determined as the position of the target position;

[0119] The end position of the acupuncture robot in the local camera coordinate system is:

[0120]

[0121] in, is the terminal position of the acupuncture robot at time k, is the local camera coordinate system {L C} is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the local coordinate, is the terminal posture of the acupuncture robot at time k;

[0122] When the local camera fails to detect the target position, the global coordinates of the target position in the global camera coordinate system are determined; the end position and posture of the acupuncture robot are determined according to the global coordinates and the velocity level kinematic equation, and the obtained end position and posture are substituted into the joint angular velocity equation, and the substitution result is integrated, and the integrated result is used as the position and posture of the target position in the global camera coordinate system;

[0123] The end position of the acupuncture robot in the global camera coordinate system is:

[0124]

[0125] in, is the terminal position of the acupuncture robot at time k, The global camera coordinate system {G C} is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the global coordinate.

[0126] S110: planning a plurality of broken line trajectories for the path between the initial position and the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories.

[0127] It can be understood that rough positioning is performed based on the initial position and the target position, and multiple segments of broken line trajectories are planned. Each segment of the broken line trajectory can be a straight line, and the path of the straight line is relatively fixed, which can improve the trajectory stability of the acupuncture robot.

[0128] Furthermore, planning a multi-segment broken line trajectory for the path between the initial position and the target position includes the following steps:

[0129] Establishing a discrete joint space model at time k for the acupuncture robot;

[0130] The discrete joint space model is:

[0131]

[0132] in, represents the joint angles of the acupuncture robot, represents the unit state matrix, represents the control matrix, T s represents the discrete time interval, u represents the joint angular velocity of the acupuncture robot, u is the control quantity,

[0133] The augmented dynamic equation of the discrete joint space model is:

[0134]

[0135] in, is the preset reference joint angle, is the target joint angle, is the tracking error between the actual joint angle and the reference joint angle at time k, and the state quantity of the discrete joint space model is ξ k =[δq k T ,ek T ] T ;

[0136] Establishing a piecewise constant tracking model based on the discrete joint space model;

[0137] The tracking model is:

[0138]

[0139] Among them, L v is the first objective function, is the incremental control sequence defined on the field of view N, and is a positive definite weight matrix, is a convex polygon constraint set, represents the maximum output allowed set;

[0140] The constraints of the tracking model are:

[0141] T s J i (q i ) k+i -S di =0;

[0142] Among them, u k+i represents the control amount corresponding to the i-th field of view, J i represents the Jacobian matrix corresponding to the i-th field of view, S di is the target trajectory tracking value corresponding to the moment;

[0143] The tracking model including the constraints is:

[0144]

[0145] stvel-con;

[0146] Among them, L Sp is the second objective function, and w is the trajectory tracking weight; it should be noted that the “vel-con” in each expression of the embodiment of the present application is the same as the expression contained in the “vel-con” in the most original tracking model;

[0147] The tracking model including the constraint conditions is used to plan a multi-segment broken line trajectory for the path from the initial position to the target position.

[0148] Furthermore, controlling the movement of the acupuncture robot according to the multiple segments of the broken line trajectory comprises the following steps:

[0149] Planning a first desired trajectory of a plurality of segments of the broken line trajectories;

[0150] Updating the tracking model including the constraint condition to obtain a first tracking model;

[0151] Substituting each segment of the first desired trajectory into the first tracking model, and controlling the acupuncture robot to move;

[0152] The first tracking model is:

[0153]

[0154] Among them, L SR is the third objective function, J Tk is the Jacobian matrix of the acupuncture robot with n degrees of freedom, w r is the first trajectory tracking weight, J Rxy represents the sub-Jacobian matrix corresponding to the roll and pitch degrees of freedom; q n ,q n-1 S represents the joint angle of the acupuncture actuator with 2 degrees of freedom at the end; d1 is the first desired trajectory;

[0155]

[0156] Wherein, P0 is the initial position, O is the target position, W is the right-angle turning point, and the path from the initial position to the target position is P0→W→O; λ is the planning step size, N is the control field of view, ρ is a positive integer, c is the number of planning times of the P0→W or W→O segment, and ∈ is an adjustable positive number; when ||PW||2<∈, the trajectory of P0→W is switched to W→O.

[0157] S120: Determine the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtain the external force acting on the needle and the length of the needle.

[0158] It is understandable that the position of the acupuncture robot when it is at the target position can be determined based on the joint angle data, and then the position of the needle set at the end of the acupuncture robot can be determined, including the position of the needle tip and the needle head. Moreover, this embodiment can also obtain the external force on the needle through the sensor; it can also obtain the original length of the needle before deformation.

[0159] S130: Calculate the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle.

[0160] Considering that the needle may bend after acupuncture due to the influence of external force, acupuncture under bending will reduce the accuracy and safety of acupuncture. Therefore, to solve this problem, this embodiment can first calculate the bending angle of the needle based on the current position of the needle tip, the current position of the needle head, the external force and the length of the needle, thereby providing basic data for subsequent bending correction.

[0161] Further, the bending angle of the needle is calculated according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle, comprising the following steps:

[0162] A spherical surface is established by taking the target position as the center of the sphere and the length of the needle as the radius;

[0163] Determining an expected position on the spherical surface, and determining the coordinates of the expected position according to the current position of the needle tip, the current position of the needle head, the external force and the radius;

[0164] The bending angle of the needle is calculated using an arc cosine function according to the coordinates of the current position of the needle tip, the coordinates of the current position of the needle head, and the coordinates of the expected position.

[0165] S140: Controlling the acupuncture robot to perform acupuncture according to the bending angle.

[0166] Specifically, this embodiment can perform trajectory correction according to the bending angle of the needle to reduce the bending angle during acupuncture.

[0167] Further, controlling the acupuncture robot to perform acupuncture according to the bending angle comprises the following steps:

[0168] Planning the expected position of the needle tip at the next moment according to the bending angle;

[0169] generating a second expected trajectory according to the expected position;

[0170]

[0171] Among them, S d2 is the second expected trajectory, M is the expected position of the needle tip at the next moment; P′ is the current position of the needle tip; 0p M is the coordinate of the expected position of the needle tip at the next moment relative to the coordinate system of the needle tip; 0 p P′ is the coordinate of the current position of the needle tip relative to the coordinate system of the needle tip;

[0172] Updating the tracking model including the constraint condition to obtain a second tracking model;

[0173] The second tracking model is:

[0174]

[0175] Among them, L AR is the fourth objective function; J′ Tk is the linear velocity Jacobian matrix corresponding to the needle's coordinate system {E}, w a Tracking weight for the second trajectory;

[0176] The second expected trajectory is substituted into the second tracking model, and the acupuncture robot is controlled to perform acupuncture.

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

[0178] The operation of the acupuncture robot is mainly divided into two steps: the first step is that the acupuncture robot performs trajectory tracking control before acupuncture according to the positioning information provided by the visual system; the second step is that the acupuncture robot performs safe force / position mixed control according to accurate acupoint measurement information. During the entire acupuncture robot operation process, there is currently no relevant research on the operational safety control of the intelligent acupuncture robot by imitating traditional Chinese medicine techniques.

[0179] Before acupuncture, the acupuncture robot needs to consider a series of strict environmental constraints when moving from the initial posture (position and posture) to the target posture for acupuncture. The model predictive control method based on multi-objective constraints is a common and effective method to complete this task. However, most related studies generate the trajectory between the robot and the target point through trajectory optimization. Therefore, the trajectory is uncertain and its reliability is affected by the accuracy of sensors and perception. In the process of trajectory optimization to reach the desired preparatory posture, the acupuncture robot needs to have multiple hard / soft complementary constraints or explicit contact models. This embodiment incorporates contact force and joint state into the optimization, which can plan the consistent motion of contact dynamics for the complex acupuncture robot's dexterous operation.

[0180] During the contact process, the actuator of acupuncture will interact with the human body's tissues. How to achieve safe operation on the human body through force and position control is a major technical challenge. Considering that the acupuncture needles in traditional Chinese medicine are flexible, it is very important to control the deformation of the needles during the contact process. However, most studies only stay at the modeling of deformable needles, and how to control the deformation in real time is still under research.

[0181] In order to solve the above problems, this embodiment builds an intelligent acupuncture robot system, and proposes an acupuncture robot modeling and control method that integrates bent needle compensation and traditional Chinese medicine techniques. It can explicitly perform safe trajectory planning in Cartesian space to achieve obstacle avoidance, and achieve accurate, efficient and robust trajectory tracking control through model predictive control, and control the force and deformation of the needle during acupuncture to achieve safety and accuracy of contact operations. In order to achieve high-fidelity planning, the bending deformation and orientation of the needle are sensed based on the beam force model, and the bending correction is achieved by performing trajectory planning and tracking control on the end of the acupuncture robot.

[0182] Specifically, this embodiment may include:

[0183] 1. Modeling and positioning of acupuncture robot.

[0184] 1.1 Acupuncture robot modeling.

[0185] Assume that the joint angle of the n-DOF acupuncture robot is q = [q1, q2, L, q n ] T , {0}~{n} are the corresponding joint coordinate systems (the origin of {n} is located at the needle tip at the end of the acupuncture robot), i T j represents the homogeneous transformation matrix of {j} relative to {i}. According to the chain rule, the kinematic equation of the acupuncture robot can be written as:

[0186] 0 T n (q1,q 2, L,q n )= 0 T1 1 T2L n-1 T n (1)

[0187] Let the Jacobian matrix from joint space to operation space be J n , the three-dimensional position vector of the end in the {0} system is 0 p n =[p x ,p y ,p z ] T , p x , p y , p z Represent the coordinate components of the x, y, and z axes respectively, and the corresponding end posture expressed in Euler angles is α, β, and γ represent the roll, pitch, and yaw components of the Euler angles, respectively. Therefore, the velocity-level kinematic equation of the acupuncture robot can be expressed as:

[0188]

[0189] in, for The derivative of for The derivative of represents the derivative of the joint angle q.

[0190] According to formula (2), the joint angular velocity of the acupuncture robot is:

[0191]

[0192] in, is the generalized inverse matrix of the acupuncture robot.

[0193] 1.2 Acupuncture point recognition by acupuncture robot.

[0194] Assume {G C} and {L C} respectively represent the global camera coordinate system and the end-point hand-eye camera coordinate system (local camera coordinate system). C The homogeneous transformation matrix of} relative to {0} is {L C} is fixed on the end acupuncture smart mechanism, so {L C} is the homogeneous transformation matrix of the terminal coordinate system {n} Fixed, so {L C The homogeneous transformation matrix of} relative to {0} can be described as:

[0195]

[0196] Acupuncture point at the end of the hand-eye camera {L C The three-dimensional coordinates under} are The acupoints are located in the global coordinate system {G C The three-dimensional coordinates under} are The first stage of trajectory tracking is to guide the acupuncture robot to the area of ​​interest through the global camera, and the local camera is positioned in real time to improve the accuracy when the acupuncture mechanism approaches the acupuncture point. The terminal position of the acupuncture robot at time k is In order to improve the stability of visual positioning, this embodiment always maintains γ = -π during the planning process of rough positioning. When the end hand-eye camera accurately measures the three-dimensional coordinates of the acupuncture point When , the position of the acupoint can be expressed as:

[0197]

[0198] When the acupoint image is beyond the field of view of the hand-eye camera, Replaced with the positioning result of the global camera Will Substitute into formula (3) to obtain the joint angular velocity of the acupuncture robot: Then, by integrating

[0199] Define the coordinate system at the acupuncture head as {E}, let z′ be the z-axis unit vector of {E}, and θ be 0 F s When the tip of the needle touches the human tissue, the force / torque measured by the 6-dimensional force sensor at the end of the acupuncture robot is expressed as in E F s =[F x ,F y ,F z ] T , F x ,F y ,F z Respectively express force E The x, y, and z components of F in {E}, E τ s =[τ x ,τ y ,τ z ] T , τ x , τ y , τ z Respectively represent the torque τ z In the x, y, and z components of {E}. When the needle is inserted vertically, ideally F x =F y = 0, but the complex tissues of the human body are prone to displacement. E F s When the needle tip is offset from z′, it will bend according to the cantilever beam principle. The size of θ determines the degree of bending. E F s Determines the direction of bending, such as Figure 2 During acupuncture, if this curvature is not corrected, the curvature will become greater as the needle is inserted deeper, reducing the accuracy of acupuncture.

[0200] 2. Trajectory tracking control before acupuncture.

[0201] 2.1 Modeling of model-free control systems.

[0202] Assuming that the n-DOF acupuncture robot is controlled by the joint angular velocity, the discrete joint space model of the acupuncture robot at time k can be expressed as:

[0203] q k+1 =Aq k +Buk (6)

[0204] in, represents the joint angles of the acupuncture robot, represents the unit state matrix, represents the control matrix, T s represents a discrete time interval, control quantity Represents the joint angular velocity of the acupuncture robot.

[0205] make:

[0206] δq k =q k -q k-1

[0207] δu k =u k -u k-1 (7)

[0208] definition is the preset reference joint angle, is the target joint angle, and the tracking error between the actual joint angle and the reference value at time k is The state quantity of the discrete joint space model is ξ k =[δq k T ,e k T ] T , so the augmented dynamic equation of the discrete joint space model is:

[0209]

[0210] In order to improve the noise immunity of the discrete joint space model, a model-free control framework based on velocity form is designed. Its tracking model in the piecewise constant reference can be expressed as:

[0211]

[0212] in, is the incremental control sequence defined on the field of view N, and is a positive definite weight matrix, is a convex polygon constraint set, Represents the maximum allowed set of outputs.

[0213] For the trajectory tracking task of the acupuncture robot, It can be a steady-state reference for the segmented trajectory, but it is difficult to balance the number of trajectory segments and the accuracy of trajectory tracking. The trajectory controlled by equation (9) is uncertain between the set points and difficult to constrain. This embodiment adds the constraint condition of operation space tracking to equation (9):

[0214] T s J i (q i ) k+i -S di =0 (10)

[0215] Among them, u k+i represents the control amount corresponding to the i-th field of view, J i represents the Jacobian matrix corresponding to the i-th field of view, S di is the target trajectory tracking value corresponding to that moment.

[0216] Formula (10) unifies the control step size and the tracking accuracy of the target trajectory, eliminating the uncertainty of formula (9) in trajectory tracking. Considering the rolling horizon principle of MPC, in order to improve the robustness of the controller, this embodiment adds the trajectory tracking constraint as a soft constraint to the objective function and proposes a model-free control method based on trajectory constraints, namely:

[0217]

[0218] stvel-con(11)

[0219] Where w is the trajectory tracking weight.

[0220] 2.2 Trajectory tracking control under coarse positioning, as shown in the example Figure 3 shown.

[0221] According to the working scenario of the acupuncture robot, the end of the acupuncture robot is planned to move to a safe position above the designated acupuncture point along a broken line trajectory, such as Figure 4 As shown in the figure. At the initial moment, the end position of the acupuncture robot is P0, the acupoint position measured by the visual system is O, and the right-angle inflection point W can be determined based on P0 and O. Therefore, the trajectory of the end in the rough planning process is P0→W→O. Due to the existence of tracking error, the actual end position P may deviate from the expected trajectory. Assuming that P is The projection point on is P h ,exist The projection point on is P v , the expected planning point at the next moment is M, then:

[0222]

[0223] Among them, λ is the planning step size, and:

[0224]

[0225] Where N is the control field of view, ρ is a positive integer, c is the number of planning times of P0→W or W→O segment, and ∈ is a small positive number.

[0226] At the next moment, the expected trajectory S of the acupuncture robot terminal planning d for:

[0227]

[0228] When ||PW||2<∈, switch the trajectory from P0→W to W→O.

[0229] Therefore, the model-free control based on trajectory constraints under coarse positioning can be established as:

[0230]

[0231] Among them, J Tk is the Jacobian matrix of the n-DOF acupuncture robot, w r is the tracking weight of the rough planning, J Rxy represents the sub-Jacobian matrix corresponding to the roll and pitch degrees of freedom. (q n ,q n-1 Represents the acupuncture actuator with 2 degrees of freedom at the end. The last constraint indicates that it is not needed in the rough planning process.

[0232] 2.3 Trajectory tracking control based on bending perception (precise positioning). The trajectory tracking control diagram under precise positioning is shown in the figure Figure 5 shown.

[0233] Considering that the bending of the needle will lead to inaccurate terminal posture corresponding to {n}, the terminal planning point of this stage selects the acupuncture head P′, and the bending of the needle is corrected by spherical planning for point P′, as shown in Figure 6 Assume that the center of the ball is located at the end position P without considering the bending of the needle, the radius of the ball is the length r of the needle, and the expected point of spherical planning is H, so the homogeneous transformation matrix corresponding to the position of point P′ is:

[0234]

[0235] in 0 R E represents the rotation transformation matrix from {0} to {E}, 0 p E Represents the three-dimensional coordinates of point P' relative to {0}.

[0236] Then we have:

[0237]

[0238] and:

[0239]

[0240] therefore:

[0241]

[0242] in, 0 p H Represents the three-dimensional coordinates of point H.

[0243] set up and The angle is θ, k = [k x ,k y ,k z ] is the unit cross product vector, then the rotation transformation matrix from {E} to {H} is:

[0244] E R H =I3+sinθ·D+(1-cosθ)D 2 (twenty two)

[0245] in,

[0246]

[0247] Change function.

[0248] The trajectory planned in this stage is an arc Since the error point P′ deviates from the spherical trajectory, the point P′ is projected onto the spherical surface is P r ′, its three-dimensional coordinates can be expressed as:

[0249]

[0250] Will After deflecting λθ around P, we get The corresponding rotation transformation matrix is 0 R P′2M ,at this time:

[0251]

[0252] Therefore, the expected trajectory of the terminal plan is:

[0253]

[0254] S d The tracking error is added as a soft constraint to the objective function, and the TCMPC model of this stage is obtained as follows:

[0255]

[0256] stvel-con(27)

[0257] Among them J T ' k is the linear velocity Jacobian matrix corresponding to {E}, w a is the trajectory tracking weight.

[0258] The overall trajectory planning diagram is as follows: Figure 7 As shown, the control process includes the following steps:

[0259] First, the position data P of the end of the acupuncture robot is calculated based on the collected joint angle data, P0 is initialized, O is initialized to be equal to P0, and the visual positioning results of the acupuncture points by the global camera and the local camera are collected at a certain frequency. and Set the threshold ∈, and then follow Figure 3 The coarse positioning pre-planning process shown controls the movement of the acupuncture robot until the acupuncture robot reaches the acupuncture point.

[0260] Then, the terminal acupuncture mechanism performs acupuncture manipulation, calculates the position data of the terminal of the acupuncture robot (i.e., the needle tip) P and the position data of the acupuncture head P′ according to the collected joint angle data, and collects the force data of the six-dimensional force sensor at a certain frequency. E F s , then according to Figure 5 The precise positioning trajectory planning process shown controls the motion of the acupuncture robot to correct the bending deformation of the needle.

[0261] This embodiment proposes a model predictive control method that integrates bending needle compensation and acupuncture techniques by sensing the bending deformation of the needle during acupuncture by the acupuncture robot, and constructs a trajectory planning and control system for the acupuncture robot. Before acupuncture, high-precision positioning and control of the acupuncture robot are achieved through a trajectory tracking method based on multi-objective constraints. Subsequently, by establishing a force model and a kinematic model, a control method for the acupuncture robot that takes bending deformation into consideration is derived, so that it can sense and correct the bending deformation during acupuncture in a complex force environment, ensuring the safety and accuracy of the acupuncture operation. The experimental results show that this method can effectively correct the bending of the needle during acupuncture, improve the accuracy of the acupuncture trajectory and the robustness of the control, and is suitable for the intelligent operation of the acupuncture robot and the safe control during acupuncture.

[0262] Reference Figure 8 The embodiment of the present application also provides an acupuncture robot bending correction control device, which can implement the above-mentioned acupuncture robot bending correction control method, and the device includes:

[0263] A position determination unit, used to determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; and determine the position to be acupunctured as the target position;

[0264] A first control unit, used for planning a plurality of broken line trajectories for a path from the initial position to the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories;

[0265] a data determination unit, for determining the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtaining the external force applied to the needle and the length of the needle;

[0266] a bending determination unit, configured to calculate the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle;

[0267] The second control unit is used to control the acupuncture robot to perform acupuncture according to the bending angle.

[0268] It can be understood that the contents of the above method embodiments are all applicable to the present device embodiments, the functions specifically implemented by the present device 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.

[0269] The embodiment of the present application also provides an electronic device, the electronic device includes a memory and a processor, the memory stores a computer program, and the processor implements the above-mentioned acupuncture robot bending correction control method when executing the computer program. The electronic device can be any intelligent terminal including a tablet computer, a car computer, etc.

[0270] It can be understood that the contents of the above method embodiments are all applicable to the present device embodiments, the functions specifically implemented by the present device 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.

[0271] See also Fig. 9 , Fig. 9 The hardware structure of an electronic device of another embodiment is illustrated, and the electronic device includes:

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

[0273] The memory 902 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 902 can store an operating system and other application programs. When the technical solution provided in the embodiment of this specification is implemented by software or firmware, the relevant program code is stored in the memory 902, and the processor 901 calls and executes the acupuncture robot bending correction control method of the embodiment of this application;

[0274] Input / output interface 903, used to implement information input and output;

[0275] Communication interface 904, used to realize communication interaction between the device and other devices, which can be realized by wired mode (such as USB, network cable, etc.) or wireless mode (such as mobile network, WIFI, Bluetooth, etc.);

[0276] A bus 905 that transmits information between various components of the device (e.g., the processor 901, the memory 902, the input / output interface 903, and the communication interface 904);

[0277] The processor 901 , the memory 902 , the input / output interface 903 and the communication interface 904 are connected to each other in communication within the device via a bus 905 .

[0278] An embodiment of the present application also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the above-mentioned acupuncture robot bending correction control method is implemented.

[0279] It can be understood that the contents of the above method embodiments are all applicable to the present storage medium embodiments, the functions specifically implemented by the present storage medium 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.

[0280] 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 disposed 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.

[0281] The embodiments described in the embodiments of the present application are intended to more clearly illustrate the technical solutions of the embodiments of the present application and do not constitute a limitation on the technical solutions provided in the embodiments of the present 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 the present application are also applicable to similar technical problems.

[0282] Those skilled in the art will appreciate 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.

[0283] The device embodiments described above are merely illustrative, and the units described as separate components may or may not be physically separated, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

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

[0285] 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 sequence. It should be understood that the data used in this way can be 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" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0286] It should be understood that in the present 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 objects associated before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single 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.

[0287] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the above units is only a logical function division. There may be other division methods in actual implementation, such as multiple units 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 devices or units, which can be electrical, mechanical or other forms.

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

[0289] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0290] If the integrated unit is implemented in the form of a software functional unit 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, 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, and the computer software product is stored in a storage medium, including multiple instructions to enable 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 (Read-Only Memory, referred to as ROM), random access memory (Random Access Memory, referred to as RAM), disk or optical disk and other media that can store programs.

[0291] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but the scope of the rights of the present invention is not limited thereto. Any modification, equivalent substitution and improvement made by a person skilled in the art without departing from the scope and essence of the present invention should be within the scope of the rights of the present invention.

Claims

1. A bending correction control method for an acupuncture robot, characterized in that: The method comprises the following steps: Determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; determine the position to be acupunctured as the target position; Planning a plurality of broken line trajectories for a path between the initial position and the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories; Determine the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtain the external force acting on the needle and the length of the needle; Calculating the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle; Controlling the acupuncture robot to perform acupuncture according to the bending angle; The method of planning a multi-segment broken line trajectory for a path from the initial position to the target position comprises the following steps: Establishing a discrete joint space model at time k for the acupuncture robot; The discrete joint space model is: q k+1 =White k +This k ; in, represents the joint angles of the acupuncture robot, represents the unit state matrix, represents the control matrix, T s represents the discrete time interval, u represents the joint angular velocity of the acupuncture robot, u is the control quantity, The augmented dynamic equation of the discrete joint space model is: in, is the preset reference joint angle, is the target joint angle, is the tracking error between the actual joint angle and the reference joint angle at time k, and the state quantity of the discrete joint space model is ξ k =[δq k T ,e k T ] T ; Establishing a piecewise constant tracking model based on the discrete joint space model; The tracking model is: Among them, L v is the first objective function, is the incremental control sequence defined on the field of view N, and is a positive definite weight matrix, is a convex polygon constraint set, represents the maximum output allowed set; The constraints of the tracking model are: T s J i (q i )u k+i -S di =0; Among them, u k+i represents the control amount corresponding to the i-th field of view, J i represents the Jacobian matrix corresponding to the i-th field of view, S di is the target trajectory tracking value corresponding to the moment; The tracking model including the constraints is: stvel-con; Among them, L Sp is the second objective function, w is the trajectory tracking weight; Planning a multi-segment broken line trajectory for a path from the initial position to the target position using the tracking model including the constraint conditions; The method of controlling the movement of the acupuncture robot according to the multiple segments of the broken line trajectory comprises the following steps: Planning a first desired trajectory of a plurality of segments of the broken line trajectories; Updating the tracking model including the constraint condition to obtain a first tracking model; Substituting each segment of the first desired trajectory into the first tracking model, and controlling the acupuncture robot to move; The first tracking model is: Among them, L SR is the third objective function, J Tk is the Jacobian matrix of the acupuncture robot with n degrees of freedom, w r is the first trajectory tracking weight, J Rxy represents the sub-Jacobian matrix corresponding to the roll and pitch degrees of freedom; q n ,q n-1 S represents the joint angle of the acupuncture actuator with 2 degrees of freedom at the end; d1 is the first desired trajectory; Wherein, P0 is the initial position, O is the target position, W is the right-angle turning point, and the path from the initial position to the target position is P0→W→O; λ is the planning step size, N is the control field of view, ρ is a positive integer, c is the number of planning times of the P0→W or W→O segment, and ∈ is an adjustable positive number; when ||PW||2<∈, the trajectory of P0→W is switched to W→O.

2. The method for controlling bending correction of an acupuncture robot according to claim 1, characterized in that: The method of determining the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot comprises the following steps: Establishing kinematic equations for the acupuncture robot; Determine the current joint angle data of the acupuncture robot according to the kinematic equation; Determining the position of the needle tip as the initial position from the current joint angle data of the acupuncture robot; The kinematic equation is: 0 T n (q1,q2,…,q n )= 0 T1 1 T2… n-1 T n ; Where q=[q1,q2,…,q n ] T is the joint angle of the acupuncture robot, {0}~{n} are the corresponding joint coordinate systems, and the origin of {n} is located at the needle tip; i T j Represents the homogeneous transformation matrix of joint coordinate system {j} relative to joint coordinate system {i}.

3. The bending correction control method of an acupuncture robot according to claim 2, characterized in that: The step of determining the position to be acupunctured as the target position comprises the following steps: Establishing the velocity level kinematic equation of the acupuncture robot according to the kinematic equation; The velocity level kinematic equation is: in, is the terminal position of the acupuncture robot, for The derivative of for The derivative of represents the derivative of the joint angle q; J n is the Jacobian matrix from joint space to operation space; 0 p n =[p x ,p y ,p z ] T is the three-dimensional position vector of the acupuncture robot end in the joint coordinate system {0}, p x , p y , p z Represent the coordinate components of the x, y, and z axes respectively; is the posture of the end of the acupuncture robot expressed in Euler angles, α, β, and γ represent the roll, pitch, and yaw components of the Euler angles, respectively; The joint angular velocity equation of the acupuncture robot is determined according to the velocity level kinematic equation as follows: in, is the generalized inverse matrix of the acupuncture robot; When the local camera disposed at the end of the acupuncture robot detects the target position, the local coordinates of the target position in the local camera coordinate system are determined; according to the local coordinates and the velocity level kinematic equation, the end position of the acupuncture robot is determined as the position of the target position; The end position of the acupuncture robot in the local camera coordinate system is: in, is the terminal position of the acupuncture robot at time k, is the local camera coordinate system {L C } is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the local coordinate, is the terminal posture of the acupuncture robot at time k; When the local camera fails to detect the target position, the global coordinates of the target position in the global camera coordinate system are determined; the end position and posture of the acupuncture robot are determined according to the global coordinates and the velocity level kinematic equation, and the obtained end position and posture are substituted into the joint angular velocity equation, and the substitution result is integrated, and the integrated result is used as the position and posture of the target position in the global camera coordinate system; The end position of the acupuncture robot in the global camera coordinate system is: in, is the terminal position of the acupuncture robot at time k, The global camera coordinate system {G C } is the homogeneous transformation matrix relative to the joint coordinate system {0}, is the global coordinate.

4. The method for controlling bending correction of an acupuncture robot according to claim 1, characterized in that: The step of calculating the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle comprises the following steps: A spherical surface is established by taking the target position as the center of the sphere and the length of the needle as the radius; Determining an expected position on the spherical surface, and determining the coordinates of the expected position according to the current position of the needle tip, the current position of the needle head, the external force and the radius; The bending angle of the needle is calculated using an arc cosine function according to the coordinates of the current position of the needle tip, the coordinates of the current position of the needle head, and the coordinates of the expected position.

5. The method for controlling bending correction of an acupuncture robot according to claim 4, characterized in that: The method of controlling the acupuncture robot to perform acupuncture according to the bending angle comprises the following steps: Planning the expected position of the needle tip at the next moment according to the bending angle; generating a second expected trajectory according to the expected position; Among them, S d2 is the second expected trajectory, M is the expected position of the needle tip at the next moment; P′ is the current position of the needle tip; 0 p M is the coordinate of the expected position of the needle tip at the next moment relative to the coordinate system of the needle tip; 0 p P′ is the coordinate of the current position of the needle tip relative to the coordinate system of the needle tip; Updating the tracking model including the constraint condition to obtain a second tracking model; The second tracking model is: stvel-con Among them, L AR is the fourth objective function; J′ Tk is the linear velocity Jacobian matrix corresponding to the needle's coordinate system {E}, w a Tracking weight for the second trajectory; The second expected trajectory is substituted into the second tracking model, and the acupuncture robot is controlled to perform acupuncture.

6. An acupuncture robot bending correction control device, characterized in that: The device comprises: A position determination unit, used to determine the position of the needle tip at the end of the acupuncture robot as the initial position according to the current joint angle data of the acupuncture robot; and determine the position to be acupunctured as the target position; A first control unit, used for planning a plurality of broken line trajectories for a path from the initial position to the target position, and controlling the movement of the acupuncture robot according to the plurality of broken line trajectories; The method of planning a multi-segment broken line trajectory for a path from the initial position to the target position comprises the following steps: Establishing a discrete joint space model at time k for the acupuncture robot; The discrete joint space model is: q k+1 =White k +This k ; in, represents the joint angles of the acupuncture robot, represents the unit state matrix, represents the control matrix, T s represents the discrete time interval, u represents the joint angular velocity of the acupuncture robot, u is the control quantity, The augmented dynamic equation of the discrete joint space model is: in, is the preset reference joint angle, is the target joint angle, is the tracking error between the actual joint angle and the reference joint angle at time k, and the state quantity of the discrete joint space model is ξ k =[δq k T ,e k T ] T ; Establishing a piecewise constant tracking model based on the discrete joint space model; The tracking model is: Among them, L v is the first objective function, is the incremental control sequence defined on the field of view N, and is a positive definite weight matrix, is a convex polygon constraint set, represents the maximum output allowed set; The constraints of the tracking model are: T s J i (q i )u k+i -S di =0; Among them, u k+i represents the control amount corresponding to the i-th field of view, J i represents the Jacobian matrix corresponding to the i-th field of view, S di is the target trajectory tracking value corresponding to the moment; The tracking model including the constraints is: stvel-con; Among them, L Sp is the second objective function, w is the trajectory tracking weight; Planning a multi-segment broken line trajectory for a path from the initial position to the target position using the tracking model including the constraint conditions; The method of controlling the movement of the acupuncture robot according to the multiple segments of the broken line trajectory comprises the following steps: Planning a first desired trajectory of a plurality of segments of the broken line trajectories; Updating the tracking model including the constraint condition to obtain a first tracking model; Substituting each segment of the first desired trajectory into the first tracking model, and controlling the acupuncture robot to move; The first tracking model is: Among them, L SR is the third objective function, J Tk is the Jacobian matrix of the acupuncture robot with n degrees of freedom, w r is the first trajectory tracking weight, J Rxy represents the sub-Jacobian matrix corresponding to the roll and pitch degrees of freedom; q n ,q n-1 S represents the joint angle of the acupuncture actuator with 2 degrees of freedom at the end; d1 is the first desired trajectory; Wherein, P0 is the initial position, O is the target position, W is the right-angle turning point, and the path from the initial position to the target position is P0→W→O; λ is the planning step size, N is the control field of view, ρ is a positive integer, c is the number of planning times of P0→W or W→O segment, and ∈ is an adjustable positive number; when ||PW||2<∈, switch the trajectory of P0→W to W→O; a data determination unit, for determining the position of the needle tip and the corresponding position of the needle head according to the joint angle data when the acupuncture robot is at the target position, and obtaining the external force applied to the needle and the length of the needle; a bending determination unit, configured to calculate the bending angle of the needle according to the current position of the needle tip, the current position of the needle head, the external force and the length of the needle; The second control unit is used to control the acupuncture robot to perform acupuncture according to the bending angle.

7. An electronic device, characterized in that: The electronic device comprises 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 5 when executing the computer program.

8. 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 5 is implemented.

Images