Robot control method based on trend perception

Abstract

The invention provides a robot control method based on trend perception, and the robot is used for treating a subject, and comprises the steps: S1, obtaining a target motion track of at least one mechanical arm, and obtaining an actual trend parameter of a treatment part, the target motion trail of the at least one mechanical arm is the same as the target motion trail of the treatment part corresponding to the mechanical arm; s2, an auxiliary force field is applied according to the target motion trail and the actual trend parameters, the robot provides auxiliary control for the treatment part of the subject based on the auxiliary force field, and the actual motion trail of the treatment part is corrected; and S3, at least one mechanical arm is made to move according to the target track based on admittance control. According to the robot control method based on trend perception provided by the invention, cooperation among a doctor, a robot and a subject can be improved, and compliance control is realized.

Description

Technical Field

[0001] The present invention belongs to the field of mechanical control, and in particular relates to a robot control method based on trend perception. Background Art

[0002] With the development of robotics technology, a large number of robots are now being used in the medical field to improve the efficiency, precision, and safety of medical services. Medical robots include those used for surgery assistance, telemedicine, and rehabilitation therapy, with robots used for assisting surgery or treatment being particularly common.

[0003] Medical robots usually achieve precise posture control through continuous trajectory control, torque control, intelligent control, etc. By controlling the movement of the robotic arm with high precision, the robot can perform precise treatment operations. Its research and development reflects the cross-integration of multiple disciplines such as mechanics, medicine, and computers. Its research and development details reflect the development trend of quantitative and standardized "integration of medicine and engineering".

[0004] However, existing medical robots can usually only move along a fixed trajectory, or intelligently identify the position coordinates that need to be moved at the next moment through image recognition and other methods, without taking into account the displacement of the subject to be treated. Moreover, if the robot is used in a vehicle, when the vehicle is bumpy, the body shape and posture of the subject to be treated will change. If the medical robot still moves according to a fixed trajectory or only adjusts the position of the end of the robotic arm according to the image recognition results, the treatment accuracy will be greatly reduced, resulting in a reduced treatment effect.

[0005] Based on the above, this application provides a technical solution to solve the above technical problems. Summary of the Invention

[0006] In response to the scenario in which conventional medical robots suffer reduced treatment accuracy when used in highly unstable environments, the present invention provides a robot control method based on trend perception. The robot includes at least a plurality of robotic arms, each of which has a distal end that grips a therapeutic instrument to treat at least one treatment area of ​​a subject. The method comprises the following steps:

[0007] Step S1: obtaining a target motion trajectory of at least one robotic arm and obtaining actual trend parameters of a treatment site, wherein the target motion trajectory of the at least one robotic arm is the same as the target motion trajectory of the treatment site corresponding to the robotic arm;

[0008] Step S2: applying an auxiliary force field according to the target motion trajectory and the actual trend parameter, wherein the robot provides auxiliary control for the treatment part of the subject based on the auxiliary force field to correct the actual motion trajectory of the treatment part;

[0009] Step S3: enabling at least one of the robotic arms to move according to the target trajectory based on admittance control.

[0010] In a specific embodiment of the present invention, in step S3, the admittance control includes:

[0011]

[0012] Among them, F ext ∈R 6×1 is the combination of the interaction force and torque between the treatment site and the end of the robotic arm, F assist ∈R 6 ×1 is the combination of interaction force and interaction torque, x out ∈R 6×1 and x des ∈R 6×1 are the actual position and expected position of the end of the manipulator that need to be controlled, respectively. M is the mass matrix, D is the damping matrix, K is the stiffness matrix, x, y, z and rx, ry, rz are the position and Euler angle of the end of the manipulator, respectively. represents the joint angular velocity, J(q)∈R 6×6 is the Jacobian matrix of the robot arm, is the joint angular velocity of the force input to the robot, is the angular velocity of the end of the robotic arm that needs to be controlled.

[0013] In a specific embodiment of the present invention, the admittance control further includes:

[0014]

[0015] F ah =F at +F an +F h ,

[0016]

[0017] Among them, F an ∈R 3×1 and F at ∈R 3×1 are normal auxiliary force and tangential auxiliary force respectively, F h ∈R 3×1 and T h ∈R 3×1 are the interaction force and interaction moment, F ah ∈R 3×1 is the resultant force of the auxiliary force and the interaction force, M L ∈R 3×3 and D L ∈R 3×3are the mass matrix and damping matrix of the position admittance controller respectively; K is the linear speed of the end of the robot arm that needs to be controlled; θ ∈R 3×3 and D θ ∈R 3×3 are the stiffness matrix and damping matrix of the attitude admittance controller, θ∈R 3×1 is the actual Euler angle at the end of the robotic arm, θ d ∈R 3×1 Represents the target Euler angle.

[0018] In a specific embodiment of the present invention, the actual trend parameters of the treatment site include at least: treatment site position, tangential motion ability, normal motion ability, and normal motion trend, and the auxiliary force field includes a normal auxiliary force field and a tangential auxiliary force field;

[0019] The tangential direction represents a motion direction that is tangent to the current position of the target motion trajectory and is the same as the motion direction of the target motion trajectory, and the normal direction is perpendicular to the tangential direction.

[0020] In a specific embodiment of the present invention, the auxiliary force field in step S2 includes:

[0021] Step S2.1, obtaining a treatment site position and a calibration mode corresponding to the treatment site position;

[0022] Step S2.2, obtaining tangential motion capability, normal motion capability, and normal motion trend;

[0023] Step S2.3, determining a normal auxiliary force field according to the normal motion trend and the normal motion capability;

[0024] Step S2.4, determining a tangential auxiliary force field according to the tangential motion capability;

[0025] Step S2.5: Correct the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field.

[0026] In a specific embodiment of the present invention, step S2.1 includes: dividing the area around the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory;

[0027] in:

[0028] When 0≤d≤R s , the treatment part is located in the first control area, and the trajectory of the treatment part is corrected based on the first correction mode;

[0029] When R s ≤d≤R m, the treatment part is located in the second control area, and the trajectory of the treatment part is corrected based on the second correction mode;

[0030] When d>R m , the treatment part is located in the third control area, and the trajectory of the treatment part is corrected based on the third correction mode;

[0031] d represents the distance between the treatment site and the target motion trajectory, R s Indicates the boundary between the first control area and the second control area, R m Indicates the boundary between the second control area and the third control area, R m >R s .

[0032] In one embodiment of the present invention, the distance between the treatment site and the target motion trajectory is the Euclidean distance between the two:

[0033] Among them, the conditions must be met

[0034] The calibration mode is: F an =F an1 +F D ,

[0035]

[0036] F at =K at F atmax t,

[0037]

[0038] Among them, α is the curvature of the target trajectory corresponding to the minimum Euclidean distance point, U is the set of spatial points that the end of the manipulator can reach, and F an represents the normal auxiliary force, P N (x N ,y N ,z N ) represents the target motion trajectory, P act (x a ,y a ,z a ) indicates the location of the treatment site, F an1 ∈R 3×1 and F D ∈R 3×1 They represent the stiffness term and damping term of the normal auxiliary force, respectively, where the damping term F D This is to ensure that the normal auxiliary force is equal to the damping coefficient K D ∈R 3×3 Attenuation is performed, and the normal auxiliary force field in the second control area is Kan In the third control area, the normal auxiliary force field increases with the deviation a·K an The speed increases, a is the coefficient of change, n∈R 3×1 Indicates the direction of the normal force, F atmax ∈R 3×1 is the maximum tangential auxiliary force, K at is the tangential auxiliary coefficient, t∈R 3×1 Indicates the direction of the tangential force.

[0039] In a specific embodiment of the present invention, step S2.3 includes:

[0040] Normal auxiliary force field

[0041] NCI=NMT·ER,

[0042]

[0043]

[0044] β represents the normal motion trend coefficient, NCI represents the trajectory following ability of the treatment site, ER represents the weighted mean error index, NMT represents the normal motion trend, K an_max and K an_min are the upper and lower limits of the normal stiffness coefficient, e p Indicates error, KW i Indicates the time weight coefficient, KW max and KW min Indicates the maximum weight coefficient and the minimum weight coefficient, i indicates the time frame number in the current time window, T all is the number of frames in the time window, m·ΔT represents the size of the moving time window, and ΔT is the control period of the robot controller.

[0045] In a specific embodiment of the present invention, the normal motion trend assessment includes:

[0046] Normal motion trend

[0047] is the motion direction of the target motion trajectory reference point corresponding to the current position, represents the interaction force at the treatment site, F ha ∈R 3×1 is the interaction force F at the treatment site h Force resolved into normal direction, F ht ∈R 3×1 is the interaction force F h The force resolved into tangential direction, F ha_maxThe maximum normal force output at the treatment site.

[0048] In a specific embodiment of the present invention, the tangential movement ability assessment includes:

[0049]

[0050] K at =g·TPI,

[0051]

[0052]

[0053] TP and TPI represent the tangential interaction force and transformation rate of the treatment part along the target motion trajectory, respectively. ht_max represents the upper limit of the transformation rate of the tangential interaction force, K at represents the tangential auxiliary force field coefficient, and g represents the tangential motion trend coefficient.

[0054] The present invention can bring at least one of the following beneficial effects: The present invention proposes a robot control method based on trend perception, which can analyze the current movement trend of the subject and apply adaptive auxiliary force to correct the position of the subject according to the position and movement trend of the subject's part to be treated, thereby improving the accuracy of treatment and being able to be effectively applied in unstable environments; in addition, the introduction of flexible admittance control enables the medical robot to safely and effectively conduct safe physical interactions with subjects and doctors. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The preferred implementation scheme will be described below in a clear and understandable manner with reference to the accompanying drawings to further illustrate the above characteristics, technical features, advantages and their implementation methods.

[0056] Figure 1 A schematic diagram of the steps of a robot control method based on trend perception proposed by the present invention;

[0057] Figure 2 Schematic diagram of the flexible control based on admittance in the present invention;

[0058] Figure 3 The division of the first control area, the second control area, and the third control area in the present invention;

[0059] Figure 4 Schematic diagram of the relationship between the actual motion trajectory and the applied auxiliary force field in the present invention. DETAILED DESCRIPTION

[0060] Various aspects of the present invention are described in further detail below.

[0061] Unless otherwise defined or indicated, all professional and scientific terms used herein have the same meaning as those familiar to those skilled in the art. In addition, any methods and materials similar or equivalent to those described herein may be applied to the present invention.

[0062] The following describes the terms.

[0063] Unless otherwise specified or limited, the "or" mentioned in the present invention includes the "and" relationship. The "and" is equivalent to the Boolean logic operator "AND", and the "or" is equivalent to the Boolean logic operator "OR", and "AND" is a subset of "OR".

[0064] It will be understood that although the terms "first," "second," and the like may be used herein to describe different elements, these elements should not be limited by these terms. These terms are merely used to distinguish one element from another. Thus, a first element may be referred to as a second element without departing from the teachings of the present invention.

[0065] In the present invention, the terms "comprising", "including" or "comprising" indicate that various components can be used together in the mixture or composition of the present invention. Therefore, the term "consisting mainly of..." is included in the terms "comprising", "including" or "comprising".

[0066] Unless otherwise specified or limited, the terms "connected," "connected," and "connected" in this application should be understood broadly. For example, they may refer to a fixed connection, a connection through an intermediary medium, internal communication between two components, or an interaction between two components. Those skilled in the art will understand the specific meanings of these terms in this application based on the specific circumstances.

[0067] For example, if an element (or component) is referred to as being on, coupled to, or connected to another element, the element may be directly formed on, coupled to, or connected to the other element, or there may be one or more intervening elements therebetween. In contrast, if the expressions "directly on," "directly coupled to," and "directly connected to" are used herein, then no intervening elements are indicated. Other words used to describe relationships between elements should be interpreted similarly, such as "between" and "directly between," "attached" and "directly attached," "adjacent" and "directly adjacent," etc.

[0068] It should be noted that the terms "front," "rear," "left," "right," "upper," and "lower" used in the following description refer to directions in the accompanying drawings. The terms "inner" and "outer" are used to refer to directions toward and away from, respectively, the geometric center of a particular component. It will be understood that these terms are used herein to describe the relationship of one element, layer, or region relative to another element, layer, or region as illustrated in the accompanying drawings. These terms are intended to encompass orientations of the device in addition to those depicted in the accompanying drawings.

[0069] Other aspects of the present invention will be apparent to those skilled in the art in view of the disclosure herein.

[0070] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the specific embodiments of the present invention will be described below with reference to the accompanying drawings. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings and other embodiments can be obtained based on these drawings without inventive work.

[0071] It should also be noted that the figures provided in the following embodiments are merely schematic illustrations of the basic concepts of the present application. The figures only show components relevant to the present application and are not drawn according to the number, shape, and size of components in actual implementation. In actual implementation, the type, quantity, and proportion of each component may be varied arbitrarily, and the component layout may be more complex. For example, the thickness of components in the drawings may be exaggerated for clarity.

[0072] Example

[0073] In view of the fact that the treatment accuracy of medical robots in existing technologies is reduced when they are used in unstable environments, such as Figure 1 As shown, the present invention provides a robot control method based on trend perception, wherein the robot includes at least a plurality of robotic arms, and the end of each robotic arm clamps a treatment instrument to treat at least one treatment part of a subject, comprising the following steps:

[0074] Step S1: obtaining a target motion trajectory of at least one robotic arm and obtaining actual trend parameters of a treatment site, wherein the target motion trajectory of the at least one robotic arm is the same as the target motion trajectory of the treatment site corresponding to the robotic arm;

[0075] Step S2: applying an auxiliary force field according to the target motion trajectory and the actual trend parameter, wherein the robot provides auxiliary control for the treatment part of the subject based on the auxiliary force field to correct the actual motion trajectory of the treatment part;

[0076] Step S3: enabling at least one of the robotic arms to move according to the target trajectory based on admittance control.

[0077] Preferably, the robot includes two robotic arms. In a specific embodiment, the medical robot is a moxibustion robot that assists or performs moxibustion treatment, including: determining and locating acupuncture points (i.e., treatment sites), controlling the burning process of moxa sticks, including parameters such as ignition, burning speed, and burning time, and accurately grasping the burning moxa stick at the end of the robotic arm according to the user's instructions, and applying precise thermal stimulation to the treatment acupuncture points.

[0078] In a preferred embodiment of the present invention, Figure 2 The schematic diagram of admittance-based flexible control is shown. In order to produce the mass-damping-spring interaction effect between the user and the robot during the interaction process, in step S3, the admittance control in Cartesian space includes:

[0079]

[0080] For a multi-DOF moxibustion robot system, the motion of the robotic arm is controlled by driving the joint velocity. The required input joint angular velocity needs to be calculated, including:

[0081]

[0082] Among them, F ext ∈R 6×1 is the combination of the interaction force and torque between the treatment site and the end of the robotic arm, F assist ∈R 6 ×1 is the combination of interaction force and interaction torque, x out ∈R 6×1 and x des ∈R 6×1 are the actual position and expected position of the end of the manipulator that need to be controlled, respectively. M is the mass matrix, D is the damping matrix, K is the stiffness matrix, x, y, z and rx, ry, rz are the position and Euler angle of the end of the manipulator, respectively. represents the joint angular velocity, J(q)∈R 6×6 is the Jacobian matrix of the robot arm, is the joint angular velocity of the force input to the robot, is the angular velocity of the end of the robotic arm that needs to be controlled.

[0083] It should be understood that in admittance control, since the subject's initiative in movement needs to be considered, the expected spatial linear velocity and the expected spatial linear acceleration are both 0, and there is interaction between the end of the manipulator and the user. The posture of the end of the manipulator cannot be completely fixed, nor can the end of the manipulator be uncontrolled. Therefore, the target Euler angle θ is added.d ∈R 3×1 , the admittance control further includes:

[0084]

[0085] F ah =F at +F an +F h ,

[0086]

[0087] Among them, F an ∈R 3×1 and F at ∈R 3×1 are normal auxiliary force and tangential auxiliary force respectively, F h ∈R 3×1 and T h ∈R 3×1 are the interaction force and interaction moment, F ah ∈R 3×1 is the resultant force of the auxiliary force and the interaction force, M L ∈R 3×3 and D L ∈R 3×3 are the mass matrix and damping matrix of the position admittance controller respectively; K is the linear speed of the end of the robot arm that needs to be controlled; θ ∈R 3×3 and D θ ∈R 3×3 are the stiffness matrix and damping matrix of the attitude admittance controller, θ∈R 3×1 is the actual Euler angle at the end of the robotic arm, θ d ∈R 3×1 Represents the target Euler angle.

[0088] Preferably, the actual trend parameters of the treatment site include at least: treatment site position, tangential motion ability, normal motion ability, and normal motion trend, and the auxiliary force field includes a normal auxiliary force field and a tangential auxiliary force field;

[0089] The tangential direction represents a motion direction that is tangent to the current position of the target motion trajectory and is the same as the motion direction of the target motion trajectory, and the normal direction is perpendicular to the tangential direction.

[0090] In a preferred embodiment of the present invention, the auxiliary force field in step S2 includes:

[0091] Step S2.1, obtaining a treatment site position and a calibration mode corresponding to the treatment site position;

[0092] Step S2.2, obtaining tangential motion capability, normal motion capability, and normal motion trend;

[0093] Step S2.3, determining a normal auxiliary force field according to the normal motion trend and the normal motion capability;

[0094] Step S2.4, determining a tangential auxiliary force field according to the tangential motion capability;

[0095] Step S2.5: Correct the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field.

[0096] Preferably, step S2.1 includes: dividing the area around the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory;

[0097] like Figure 3 As shown, when 0≤d≤R s , the treatment site is located in the first control area, and the trajectory of the treatment site is corrected based on the first correction mode; when R s ≤d≤R m , the treatment site is located in the second control area, and the trajectory of the treatment site is corrected based on the second correction mode; when d>R m , the treatment site is located in the third control area, and the trajectory of the treatment site is corrected based on the third correction mode; d represents the distance between the treatment site and the target motion trajectory, R s Indicates the boundary between the first control area and the second control area, R m Indicates the boundary between the second control area and the third control area, R m >R s The target trajectory is R, P S represents the starting point of the trajectory, P E Indicates the end point of the trajectory, and the auxiliary force is R S .

[0098] Among them, in order to allow the user to be as close as possible to the allowable error range and have a certain degree of freedom, the first control area is a fault-tolerant area. The position of the treatment part in this area is not corrected, and the subject's posture is not intervened in the first correction mode; when the treatment part enters the second control area, it means that the subject's position or posture has a certain deviation. At this time, a certain auxiliary force needs to be applied to the treatment part, that is, the trajectory of the treatment part is corrected based on the second correction mode; if the treatment part enters the third control area, it means that the subject's position or posture has a large deviation. At this time, a larger auxiliary force needs to be applied to quickly guide the subject back to the target motion trajectory.

[0099] Specifically, such as Figure 4As shown in , the distance d between the treatment site and the target motion trajectory is the Euclidean distance between the two:

[0100] Among them, it is necessary to satisfy that the Euclidean distance between the current point and the nearest point should be greater than the curvature radius of the nearest point on the spatial trajectory, that is:

[0101] The calibration mode is: F an =F an1 +F D ,

[0102]

[0103] F at =K at F atmax t,

[0104]

[0105] Among them, α is the curvature of the target trajectory corresponding to the minimum Euclidean distance point, U is the set of spatial points that the end of the manipulator can reach, and F an represents the normal auxiliary force, P N (x N ,y N ,z N ) represents the target motion trajectory, P act (x a ,y a ,z a ) indicates the location of the treatment site, F an1 ∈R 3×1 and F D ∈R 3×1 They represent the stiffness term and damping term of the normal auxiliary force, respectively, where the damping term F D This is to ensure that the normal auxiliary force is equal to the damping coefficient K D ∈R 3×3 Attenuation is performed to stabilize the system. The normal auxiliary force field in the second control region is K an In the third control area, the normal auxiliary force field increases with the deviation a·K an The speed increases, a is the coefficient of change, n∈R 3×1 Indicates the direction of the normal force, F atmax ∈R 3×1 is the maximum tangential auxiliary force, K at is the tangential auxiliary coefficient, t∈R 3 ×1 Indicates the direction of the tangential force, which is always the same as the direction of motion of the target trajectory.

[0106] In a specific embodiment of the present invention, step S2.3 includes:

[0107] Normal auxiliary force field

[0108] NCI=NMT·ER,

[0109]

[0110] β represents the normal motion trend coefficient, which is used to assist NCI to promote the subject's active participation in exercise. NCI represents the trajectory following ability of the treatment site. ER represents the weighted mean error index. NMT represents the normal motion trend. K an_max and K an_min are the upper and lower limits of the normal stiffness coefficient, e p Indicates error, KW i Indicates the time weight coefficient, KW max and KW min Indicates the maximum weight coefficient and the minimum weight coefficient. The maximum weight coefficient is usually 1. i represents the time frame number in the current time window, T all is the number of frames in the time window, m·ΔT represents the size of the moving time window, and ΔT is the control period of the robot controller.

[0111] β is also implemented using a moving weighted mean filter to filter out interference effects, with a time window of n·ΔT. When the subject moves toward the trajectory, the coefficient decreases rapidly, causing the strength of the normal assist force field to also decrease rapidly. When the subject moves away from the trajectory, β is 1, and the strength of the normal assist force field only changes slowly based on the NCI.

[0112] In a specific embodiment of the present invention, the normal motion trend assessment includes:

[0113] Calculate the normal motion trend based on the moving weighted mean filter method:

[0114]

[0115] is the motion direction of the target motion trajectory reference point corresponding to the current position, represents the interaction force at the treatment site, F ha ∈R 3×1 is the interaction force F at the treatment site h Force resolved into normal direction, F ht ∈R 3×1 is the interaction force F h The force resolved into tangential direction, F ha_max The maximum normal force output at the treatment site, used for normalization.

[0116] When the interaction force approaches the target trajectory, the NMT result is less than 1, and the normal force field gain is reduced; when it moves away from the trajectory, the NMT result is greater than 1, and the normal force field gain is increased.

[0117] In one embodiment of the present invention, a tangential assist force field is also applied in the tangential direction to guide the treatment area toward the target location. The magnitude of the tangential assist force can be adaptively adjusted based on the user's movement intent. The tangential assist force field is determined based on the tangential motion capability. When the tangential interaction force is large, indicating good tangential motion capability, minimal or no assist force can be provided. When the subject is unable to move, the robot is required to provide tangential assist force to guide the user's movement.

[0118] The user's tangential motion trend must be considered, and the subject's tendency to actively increase or decrease tangential output force must be analyzed. Given the same tangential interaction force, the control system should provide less assistive force for an increasing trend and more assistive force for a decreasing trend.

[0119] The tangential motion ability assessment includes tangential performance indicators obtained by comprehensively processing the tangential interaction force value and its change rate along the target trajectory based on the moving weighted mean filtering method:

[0120]

[0121] K at =g·TPI,

[0122]

[0123] TP and TPI represent the tangential interaction force and transformation rate of the treatment part along the target motion trajectory, respectively. ht_max K represents the upper limit of the tangential interaction force transformation rate and is used for normalization. at represents the tangential auxiliary force field coefficient, and g represents the tangential motion trend coefficient.

[0124] The adaptive coefficient g decreases when the subject increases the tangential interaction force, resulting in a rapid decrease in the assist force; when the subject decreases the tangential interaction force, the coefficient g is 1, and the tangential assist force gradually increases according to the TPI.

[0125] In summary, the present invention has achieved the following effects:

[0126] The present invention proposes a robot control method based on trend perception, which can analyze the current movement trend of the subject and apply adaptive auxiliary force to correct the position of the subject according to the position and movement trend of the subject's to-be-treated part, thereby improving the accuracy of treatment and being able to be effectively applied in unstable environments; in addition, compliant admittance control is introduced, so that the medical robot can safely and effectively conduct safe physical interactions with the subject and the doctor.

[0127] Based on this application, those skilled in the art will appreciate that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, an apparatus and / or method can be implemented using any number and aspects described herein. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement the apparatus and / or method.

[0128] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, and units provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same functions of the system and its various devices, modules, and units provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; the devices, modules, and units for implementing various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0129] It should be noted that the above embodiments can be freely combined as needed. The above description is only a preferred embodiment of the present invention. It should be pointed out that those skilled in the art can make several improvements and modifications without departing from the principles of the present invention, and such improvements and modifications should also be considered within the scope of protection of the present invention.

[0130] All documents mentioned in this application are incorporated herein by reference, just as if each document were incorporated herein by reference individually. It should also be understood that after reading the above disclosure, those skilled in the art may make various changes or modifications to the present invention, and that such equivalents also fall within the scope of the claims appended hereto.

Claims

1. A robot control method based on trend perception, wherein the robot comprises at least a plurality of robotic arms, each of which has a distal end holding a therapeutic device to treat at least one treatment site of a subject, characterized in that: The following steps are involved: Step S1: obtaining a target motion trajectory of at least one robotic arm end and obtaining actual trend parameters of a treatment site, wherein the target motion trajectory of the at least one robotic arm end is the same as the target motion trajectory of the treatment site corresponding to the robotic arm end; Step S2: applying an auxiliary force field according to the target motion trajectory and the actual trend parameter, wherein the robot provides auxiliary control for the treatment part of the subject based on the auxiliary force field to correct the actual motion trajectory of the treatment part; Step S3: enabling at least one of the robotic arms to move according to the target trajectory based on admittance control.

2. The robot control method based on trend perception according to claim 1, characterized in that: In step S3, the admittance control includes: Among them, F ext ∈R 6×1 is the combination of the interaction force and torque between the treatment site and the end of the robotic arm, F assist ∈R 6×1 is the combination of interaction force and interaction torque, x out ∈R 6×1 and x des ∈R 6×1 are the actual position and expected position of the end of the manipulator that need to be controlled, respectively. M is the mass matrix, D is the damping matrix, K is the stiffness matrix, x, y, z and rx, ry, rz are the position and Euler angle of the end of the manipulator, respectively. represents the joint angular velocity, J(q)∈R 6×6 is the Jacobian matrix of the robot arm, is the joint angular velocity of the force input to the robot, is the angular velocity of the end of the robotic arm that needs to be controlled.

3. The robot control method based on trend perception according to claim 2, characterized in that: The admittance control further includes: F ah =F at +F an +F h , Among them, F an ∈R 3×1 and F at ∈R 3×1 are normal auxiliary force and tangential auxiliary force respectively, F h ∈R 3×1 and T h ∈R 3×1 are the interaction force and interaction moment, F ah ∈R 3×1 is the resultant force of the auxiliary force and the interaction force, M L ∈R 3×3 and D L ∈R 3×3 are the mass matrix and damping matrix of the position admittance controller respectively; K is the linear speed of the end of the robot arm that needs to be controlled; θ ∈R 3×3 and D θ ∈R 3×3 are the stiffness matrix and damping matrix of the attitude admittance controller, θ∈R 3×1 is the actual Euler angle at the end of the robotic arm, θ d ∈R 3×1 Represents the target Euler angle.

4. The robot control method based on trend perception according to claim 3, characterized in that: The actual trend parameters of the treatment site include at least: treatment site position, tangential motion ability, normal motion ability, and normal motion trend; the auxiliary force field includes a normal auxiliary force field and a tangential auxiliary force field; The tangential direction represents a motion direction that is tangent to the current position of the target motion trajectory and is the same as the motion direction of the target motion trajectory, and the normal direction is perpendicular to the tangential direction.

5. The robot control method based on trend perception according to claim 4, characterized in that: The auxiliary force field in step S2 includes: Step S2.1, obtaining a treatment site position and a calibration mode corresponding to the treatment site position; Step S2.2, obtaining tangential motion capability, normal motion capability, and normal motion trend; Step S2.3, determining a normal auxiliary force field according to the normal motion trend and the normal motion capability; Step S2.4, determining a tangential auxiliary force field according to the tangential motion capability; Step S2.5: Correct the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field.

6. The robot control method based on trend perception according to claim 5, characterized in that: Step S2.1 includes: dividing the area around the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory; in: When 0≤d≤R s , the treatment part is located in the first control area, and the trajectory of the treatment part is corrected based on the first correction mode; When R s ≤d≤R m , the treatment part is located in the second control area, and the trajectory of the treatment part is corrected based on the second correction mode; When d>R m , the treatment part is located in the third control area, and the trajectory of the treatment part is corrected based on the third correction mode; d represents the distance between the treatment site and the target motion trajectory, R s Indicates the boundary between the first control area and the second control area, R m Indicates the boundary between the second control area and the third control area, R m >R s .

7. The robot control method based on trend perception according to claim 6, characterized in that: The distance between the treatment site and the target trajectory is the Euclidean distance between the two: Among them, the conditions must be met The calibration mode is: F an =F an1 +F D , F at =K at F atmax t, Among them, α is the curvature of the target trajectory corresponding to the minimum Euclidean distance point, U is the set of spatial points that the end of the manipulator can reach, and F an represents the normal auxiliary force, P N (x N ,y N ,z N ) represents the target motion trajectory, P act (x a ,y a ,z a ) indicates the location of the treatment site, F an1 ∈R 3×1 and F D ∈R 3×1 They represent the stiffness term and damping term of the normal auxiliary force, respectively, where the damping term F D This is to ensure that the normal auxiliary force is equal to the damping coefficient K D ∈R 3×3 Attenuation is performed, and the normal auxiliary force field in the second control area is K an In the third control area, the normal auxiliary force field increases with the deviation a·K an The speed increases, a is the coefficient of change, n∈R 3×1 Indicates the direction of the normal force, F atmax ∈R 3×1 is the maximum tangential auxiliary force, K at is the tangential auxiliary coefficient, t∈R 3×1 Indicates the direction of the tangential force.

8. The robot control method based on trend perception according to claim 7, characterized in that: The step S2.3 includes: Normal auxiliary force field NCI=NMT·ER, KW i =KW min +(KW max +KW min )i / T all , β represents the normal motion trend coefficient, NCI represents the trajectory following ability of the treatment site, ER represents the weighted mean error index, NMT represents the normal motion trend, K an_max and K an_min are the upper and lower limits of the normal stiffness coefficient, e p Indicates error, KW i Indicates the time weight coefficient, KW max and KW min Indicates the maximum weight coefficient and the minimum weight coefficient, i indicates the time frame number in the current time window, T all is the number of frames in the time window, m·ΔT represents the size of the moving time window, and ΔT is the control period of the robot controller.

9. The robot control method based on trend perception according to claim 8, characterized in that: The normal motion trend evaluation includes: Normal motion trend is the motion direction of the target motion trajectory reference point corresponding to the current position, represents the interaction force at the treatment site, F ha ∈R 3×1 is the interaction force F at the treatment site h Force resolved into normal direction, F ht ∈R 3×1 is the interaction force F h The force resolved into tangential direction, F ha_max The maximum normal force output at the treatment site.

10. The robot control method based on trend perception according to claim 9, characterized in that: The tangential movement ability assessment includes: K at =g·TPI, TP and TPI represent the tangential interaction force and transformation rate of the treatment part along the target motion trajectory, respectively. ht_max represents the upper limit of the transformation rate of the tangential interaction force, K at represents the tangential auxiliary force field coefficient, and g represents the tangential motion trend coefficient.

Images